├── .circleci └── config.yml ├── .gitignore ├── LICENSE ├── README.md ├── aws.yml ├── doc ├── building.md └── img │ └── smart_grasping_sandbox.png ├── docker ├── base │ └── Dockerfile └── latest │ └── Dockerfile ├── entrypoint.sh ├── fh_desc ├── CMakeLists.txt ├── config │ └── hand_H_gazebo_trajectory.yaml ├── launch │ └── launch_simulation.launch ├── mainpage.dox ├── meshes │ ├── omd-d18-tip.dae │ └── v2 │ │ ├── .placeholder │ │ ├── finger_base_link_simple.dae │ │ ├── finger_dist_link_simple.dae │ │ ├── finger_palm_link.dae │ │ ├── finger_prox_link_simple.dae │ │ └── palm_link_simple.dae ├── model.config ├── model.urdf └── package.xml ├── jenkins.yml ├── jupyter_notebook_config.py ├── repository.rosinstall ├── setup_gzweb.sh ├── shippable.yml ├── smart_grasp_moveit_config ├── .setup_assistant ├── CMakeLists.txt ├── config │ ├── fake_controllers.yaml │ ├── gazebo_controllers.yaml │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── ompl_planning.yaml │ └── ur10.srdf ├── launch │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── joystick_control.launch │ ├── move_group.launch │ ├── moveit.rviz │ ├── moveit_rviz.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── smart_grasping_moveit_controller_manager.launch.xml │ ├── trajectory_execution.launch.xml │ ├── ur10_moveit_controller_manager.launch.xml │ ├── ur10_moveit_sensor_manager.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml └── package.xml └── smart_grasping_sandbox ├── CMakeLists.txt ├── launch └── smart_grasping_sandbox.launch ├── models ├── building │ ├── model.config │ └── model.sdf ├── floor │ ├── model.config │ └── model.sdf ├── kinect_pilar │ ├── model.config │ └── model.sdf ├── kinect_ros │ ├── materials │ │ └── textures │ │ │ └── kinect.png │ ├── meshes │ │ └── kinect.dae │ ├── model-1_2.sdf │ ├── model-1_3.sdf │ ├── model-1_4.sdf │ ├── model.config │ ├── model.sdf │ └── thumbnails │ │ └── 0.png └── robot_pilar │ ├── model.config │ └── model.sdf ├── nodes ├── README.md ├── grasp_quality.py └── grasp_quality_entrypoint.sh ├── notebooks ├── Display grasp quality.ipynb ├── Grasp Bayesian Optimization.ipynb ├── Grasp Quality.ipynb ├── RL-Hand.ipynb ├── Smart Grasping Sandbox.ipynb ├── model-Q.h5 ├── model.h5 └── state.pckl ├── package.xml ├── setup.py ├── src └── smart_grasping_sandbox │ ├── __init__.py │ └── smart_grasper.py └── world └── smart_grasping.world /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | jobs: 3 | build: 4 | working_directory: /tmp/repository 5 | docker: 6 | - image: shadowrobot/build-tools:xenial-kinetic 7 | environment: 8 | toolset_branch: master 9 | server_type: circle 10 | ros_release_name: kinetic 11 | ubuntu_version_name: xenial 12 | used_modules: check_cache,code_style_check,check_license 13 | steps: 14 | - checkout 15 | - run: echo 'export remote_shell_script="https://raw.githubusercontent.com/shadow-robot/sr-build-tools/$toolset_branch/bin/sr-run-ci-build.sh"' >> $BASH_ENV 16 | - run: wget -O /tmp/oneliner "$( echo "$remote_shell_script" | sed 's/#/%23/g' )" 17 | - run: chown -R $MY_USERNAME:$MY_USERNAME $CIRCLE_WORKING_DIRECTORY 18 | - run: chmod 755 /tmp/oneliner 19 | - run: gosu $MY_USERNAME /tmp/oneliner "$toolset_branch" $server_type $used_modules 20 | 21 | - store_test_results: 22 | path: test_results 23 | 24 | - store_artifacts: 25 | path: code_coverage_results 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | | Check | Status | 2 | |---------------|--------| 3 | | Docker | [![Docker Automated build](https://img.shields.io/docker/automated/shadowrobot/smart_grasping_sandbox.svg)](https://hub.docker.com/r/shadowrobot/smart_grasping_sandbox) | 4 | | Landscape | [![Code Health](https://landscape.io/github/shadow-robot/smart_grasping_sandbox/master/landscape.svg?style=flat)](https://landscape.io/github/shadow-robot/smart_grasping_sandbox/master) | 5 | 6 | # smart_grasping_sandbox 7 | 8 | This is a public simulation sandbox for [Shadow's Smart Grasping System](https://www.shadowrobot.com/shadow-smart-grasping-system/). We're aiming to provide you with a simplified simulation environment to play with different challenges in an autonomous pick and place problem. 9 | 10 | This stack contains: 11 | * **doc**: some additional documentation 12 | * **fh_description**: the urdf description of the robot 13 | * **smart_grasp_moveit_config**: a MoveIt! config for the motion planning 14 | * **smart_grasping_sandbox**: the main point of entrance, includes the launch file and the main python files. 15 | 16 | ![Smart Grasping Sandbox screenshot](doc/img/smart_grasping_sandbox.png) 17 | 18 | ## Getting started 19 | 20 | The fastest way to get started is using Docker. If you haven't done so already head over to the [Docker website](https://www.docker.com/) and follow the instructions over there. Once installed you can simply fire up the pre-built Docker container (it will be downloaded automatically for you): 21 | 22 | ``` 23 | docker run -it --name sgs -p 8080:8080 -p 8888:8888 -p 8181:8181 -p 7681:7681 shadowrobot/smart_grasping_sandbox 24 | ``` 25 | 26 | Then connect your local browser to [localhost:8080](http://localhost:8080) to see the simulation and [localhost:8888](http://localhost:8888) to get access to an ipython notebook which you can tinker with. The password for the ipython notebook is `shadow`. 27 | 28 | The [ipython notebook](http://localhost:8888) contains an example of interaction with the sandbox to help you get started. If you want to check which functions are available in the `SmartGrasper` helper library, you can run from python: 29 | 30 | ```python 31 | from smart_grasping_sandbox.smart_grasper import SmartGrasper 32 | help(SmartGrasper) 33 | ``` 34 | 35 | ## Restarting the container 36 | 37 | The next time you want to restart this container you just have to run: 38 | 39 | ``` 40 | docker start sgs 41 | ``` 42 | 43 | And you will be able to connect to [localhost:8080](http://localhost:8080) and [localhost:8888](http://localhost:8888) again. 44 | 45 | If on startup you get the following error: 46 | ``` 47 | [Err] [RenderEngine.cc:734] Can't open display: :1.0 48 | ``` 49 | The camera topics will not be published. This error is caused by an erratic closure of the X server processes. In order to fix it run the following command on your host: 50 | ``` 51 | docker exec -it sgs bash 52 | ``` 53 | then run: 54 | 55 | ``` 56 | rm /tmp/.X1-lock 57 | ``` 58 | 59 | Finally restart the container. 60 | 61 | ## More advanced use 62 | 63 | A Cloud9IDE editor is also available for the more advanced user on [localhost:8181](http://localhost:8181). 64 | -------------------------------------------------------------------------------- /aws.yml: -------------------------------------------------------------------------------- 1 | settings: 2 | machine_type: small 3 | ubuntu: 4 | version: trusty 5 | ros: 6 | release: indigo 7 | docker: 8 | image: shadowrobot/build-tools 9 | tag: trusty-indigo 10 | template_project_name: template_unit_tests_and_code_coverage 11 | toolset: 12 | modules: 13 | - check_cache 14 | - code_coverage 15 | 16 | trunks: 17 | - name: master 18 | jobs: 19 | - name: code_coverage 20 | - name: style_check 21 | settings: 22 | ubuntu: 23 | version: xenial 24 | ros: 25 | release: kinetic 26 | docker: 27 | image: shadowrobot/build-tools 28 | tag: xenial-kinetic 29 | toolset: 30 | modules: 31 | - code_style_check 32 | - check_license 33 | - name: install_check 34 | settings: 35 | toolset: 36 | modules: 37 | - check_install 38 | -------------------------------------------------------------------------------- /doc/building.md: -------------------------------------------------------------------------------- 1 | 2 | ## Building the docker container 3 | First clone the repository. Then go to the root of the cloned repository (where the README, Dockerfile, etc... are) and run: 4 | 5 | ``` 6 | docker build -t smart-grasping-sandbox . 7 | ``` 8 | 9 | You can now start the container: 10 | 11 | ``` 12 | docker run -it --name sgs --entrypoint /bin/bash -p 8080:8080 -p 7681:7681 -p 8181:8181 smart-grasping-sandbox 13 | ``` 14 | -------------------------------------------------------------------------------- /doc/img/smart_grasping_sandbox.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/doc/img/smart_grasping_sandbox.png -------------------------------------------------------------------------------- /docker/base/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:indigo-desktop-full 2 | 3 | # using bash instead of sh to be able to source 4 | ENV TERM xterm 5 | RUN rm /bin/sh && ln -s /bin/bash /bin/sh 6 | 7 | RUN apt-get update && \ 8 | DEBIAN_FRONTEND=noninteractive apt-get install -y python-catkin-tools ros-indigo-moveit wget && \ 9 | echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list && \ 10 | wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - && \ 11 | apt-get update && \ 12 | apt-get remove -y gazebo2 && \ 13 | DEBIAN_FRONTEND=noninteractive apt-get install -y gazebo7 ros-indigo-gazebo7-ros-pkgs ros-indigo-gazebo7-ros-control ros-indigo-controller-manager ros-indigo-ros-controllers python-pip && \ 14 | mkdir -p /workspace/src && \ 15 | cd /workspace/ && \ 16 | source /opt/ros/indigo/setup.bash && \ 17 | catkin init 18 | 19 | COPY . /workspace/src/ 20 | 21 | RUN source /opt/ros/indigo/setup.bash && \ 22 | cd /workspace/src && \ 23 | git clone -b indigo-devel https://github.com/ros-simulation/gazebo_ros_pkgs.git && \ 24 | git clone https://github.com/shadow-robot/pysdf.git && \ 25 | git clone -b F_add_moveit_funtionallity https://github.com/shadow-robot/gazebo2rviz.git && \ 26 | git clone -b F_gazebo_7_docker https://github.com/shadow-robot/universal_robot.git && \ 27 | git clone -b F#182_partial_trajectory_mod https://github.com/shadow-robot/ros_controllers.git && \ 28 | cd /workspace/src && \ 29 | wget https://raw.githubusercontent.com/osrf/osrf-rosdep/master/gazebo7/00-gazebo7.list -O /etc/ros/rosdep/sources.list.d/00-gazebo7.list && \ 30 | rosdep update && \ 31 | rosdep install --default-yes --all --ignore-src && \ 32 | catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release 33 | 34 | # installing gzweb 35 | RUN curl -sL https://deb.nodesource.com/setup | bash - && \ 36 | DEBIAN_FRONTEND=noninteractive apt-get install -y libjansson-dev nodejs libboost-dev imagemagick libtinyxml-dev mercurial cmake build-essential xvfb 37 | 38 | RUN /workspace/src/setup_gzweb.sh 39 | 40 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y byobu nano 41 | 42 | RUN cd /root && \ 43 | git clone git://github.com/c9/core.git c9sdk && \ 44 | cd c9sdk && \ 45 | scripts/install-sdk.sh && \ 46 | sed -i -e 's_127.0.0.1_0.0.0.0_g' /root/c9sdk/configs/standalone.js 47 | 48 | 49 | RUN apt-get remove -y python-pip && \ 50 | wget https://bootstrap.pypa.io/get-pip.py && \ 51 | python get-pip.py && \ 52 | pip2 install --upgrade packaging jupyter && \ 53 | pip2 install --upgrade jupyter_contrib_nbextensions && \ 54 | jupyter contrib nbextension install --system --symlink && \ 55 | mkdir -p /root/.jupyter && \ 56 | jupyter nbextension enable toc2/main 57 | 58 | COPY jupyter_notebook_config.py /root/.jupyter/ 59 | 60 | RUN apt-get remove -y python-pip && \ 61 | wget https://bootstrap.pypa.io/get-pip.py && \ 62 | python get-pip.py && \ 63 | pip2 install --upgrade packaging jupyter 64 | 65 | RUN pip2 install --upgrade tensorflow keras h5py sklearn bokeh bayesian-optimization pandas 66 | 67 | # cleanup 68 | RUN rm -rf /var/lib/apt/lists/* 69 | 70 | # setup entrypoint 71 | COPY ./entrypoint.sh / 72 | 73 | ENTRYPOINT ["/entrypoint.sh"] 74 | CMD ["bash"] 75 | 76 | EXPOSE 8080 8888 7681 77 | -------------------------------------------------------------------------------- /docker/latest/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM shadowrobot/smart_grasping_sandbox:base 2 | 3 | RUN cd /workspace/src && \ 4 | git pull origin 5 | -------------------------------------------------------------------------------- /entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | nohup Xvfb :1 -screen 0 1024x768x16 &> xvfb.log & 5 | DISPLAY=:1.0 6 | export DISPLAY 7 | 8 | source "/workspace/devel/setup.bash" 9 | source "/usr/share/gazebo/setup.sh" 10 | 11 | roslaunch smart_grasping_sandbox smart_grasping_sandbox.launch gui:=false gzweb:=true verbose:=true & 12 | 13 | sleep 5 14 | 15 | cd ~/gzweb 16 | GAZEBO_MODEL_PATH=/workspace/src:/workspace/src/universal_robot:~/.gazebo/models:${GAZEBO_MODEL_PATH} ./start_gzweb.sh & 17 | 18 | cd ~/c9sdk 19 | node server.js --listen 0.0.0.0 --port 8181 -w /workspace/src & 20 | 21 | cd /workspace/src/smart_grasping_sandbox/notebooks 22 | jupyter notebook --ip=0.0.0.0 --allow-root 23 | 24 | -------------------------------------------------------------------------------- /fh_desc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fh_desc) 3 | find_package(catkin REQUIRED COMPONENTS urdf xacro rospy) 4 | 5 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 6 | 7 | catkin_package( 8 | DEPENDS 9 | CATKIN_DEPENDS urdf xacro rospy 10 | INCLUDE_DIRS 11 | LIBRARIES 12 | ) 13 | 14 | install(DIRECTORY meshes/ 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes) 16 | -------------------------------------------------------------------------------- /fh_desc/config/hand_H_gazebo_trajectory.yaml: -------------------------------------------------------------------------------- 1 | hand_controller: 2 | gains: 3 | H1_F1J1: 4 | p: 10.0 5 | H1_F1J2: 6 | p: 10.0 7 | H1_F1J3: 8 | p: 10.0 9 | H1_F2J1: 10 | p: 10.0 11 | H1_F2J2: 12 | p: 10.0 13 | H1_F2J3: 14 | p: 10.0 15 | H1_F3J1: 16 | p: 10.0 17 | H1_F3J2: 18 | p: 10.0 19 | H1_F3J3: 20 | p: 10.0 21 | type: effort_controllers/JointTrajectoryController 22 | joints: [H1_F1J1, H1_F1J2, H1_F1J3, H1_F2J1, H1_F2J2, H1_F2J3, H1_F3J1, H1_F3J2, H1_F3J3] 23 | allow_partial_joints_goal: true 24 | -------------------------------------------------------------------------------- /fh_desc/launch/launch_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 33 | 35 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /fh_desc/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b fh_desc is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /fh_desc/meshes/v2/.placeholder: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/fh_desc/meshes/v2/.placeholder -------------------------------------------------------------------------------- /fh_desc/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | smart_grasping_sandbox 5 | 1.0 6 | model.urdf 7 | 8 | 9 | Ugo Cupcic 10 | software@shadowrobot.com 11 | 12 | 13 | 14 | Hand H. 15 | 16 | 17 | -------------------------------------------------------------------------------- /fh_desc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | fh_desc 3 | 0.0.0 4 | fh_desc contains the description for Shadow Robot's flexible hand. 5 | Shadow Robot's software team 6 | 7 | GPLv3 8 | 9 | https://github.com/shadow-robot/fh_common 10 | https://github.com/shadow-robot/fh_common/issues 11 | 12 | Ugo Cupcic 13 | 14 | 15 | catkin 16 | 17 | 18 | urdf 19 | xacro 20 | rospy 21 | 22 | 23 | urdf 24 | xacro 25 | rospy 26 | 27 | -------------------------------------------------------------------------------- /jenkins.yml: -------------------------------------------------------------------------------- 1 | settings: 2 | ubuntu: 3 | version: trusty 4 | ros: 5 | release: indigo 6 | docker: 7 | image: shadowrobot/build-tools 8 | tag: trusty-indigo 9 | toolset: 10 | template_job_name: template_unit_tests_and_code_coverage 11 | modules: 12 | - check_cache 13 | - code_coverage 14 | 15 | trunks: 16 | - name: indigo-devel 17 | - name: kinetic-devel 18 | settings: 19 | ubuntu: 20 | version: xenial 21 | ros: 22 | release: kinetic 23 | docker: 24 | tag: xenial-kinetic 25 | 26 | branch: 27 | parent: indigo-devel 28 | -------------------------------------------------------------------------------- /jupyter_notebook_config.py: -------------------------------------------------------------------------------- 1 | # Configuration file for jupyter-notebook. 2 | 3 | # Copyright (C) 2018 Shadow Robot Company Ltd - All Rights Reserved. 4 | # Proprietary and Confidential. Unauthorized copying of the content in this file, via any medium is strictly prohibited. 5 | 6 | #------------------------------------------------------------------------------ 7 | # Application(SingletonConfigurable) configuration 8 | #------------------------------------------------------------------------------ 9 | 10 | ## This is an application. 11 | 12 | ## The date format used by logging formatters for %(asctime)s 13 | #c.Application.log_datefmt = '%Y-%m-%d %H:%M:%S' 14 | 15 | ## The Logging format template 16 | #c.Application.log_format = '[%(name)s]%(highlevel)s %(message)s' 17 | 18 | ## Set the log level by value or name. 19 | #c.Application.log_level = 30 20 | 21 | #------------------------------------------------------------------------------ 22 | # JupyterApp(Application) configuration 23 | #------------------------------------------------------------------------------ 24 | 25 | ## Base class for Jupyter applications 26 | 27 | ## Answer yes to any prompts. 28 | #c.JupyterApp.answer_yes = False 29 | 30 | ## Full path of a config file. 31 | #c.JupyterApp.config_file = u'' 32 | 33 | ## Specify a config file to load. 34 | #c.JupyterApp.config_file_name = u'' 35 | 36 | ## Generate default config file. 37 | #c.JupyterApp.generate_config = False 38 | 39 | #------------------------------------------------------------------------------ 40 | # NotebookApp(JupyterApp) configuration 41 | #------------------------------------------------------------------------------ 42 | 43 | ## Set the Access-Control-Allow-Credentials: true header 44 | #c.NotebookApp.allow_credentials = False 45 | 46 | ## Set the Access-Control-Allow-Origin header 47 | # 48 | # Use '*' to allow any origin to access your server. 49 | # 50 | # Takes precedence over allow_origin_pat. 51 | #c.NotebookApp.allow_origin = '' 52 | 53 | ## Use a regular expression for the Access-Control-Allow-Origin header 54 | # 55 | # Requests from an origin matching the expression will get replies with: 56 | # 57 | # Access-Control-Allow-Origin: origin 58 | # 59 | # where `origin` is the origin of the request. 60 | # 61 | # Ignored if allow_origin is set. 62 | #c.NotebookApp.allow_origin_pat = '' 63 | 64 | ## DEPRECATED use base_url 65 | #c.NotebookApp.base_project_url = '/' 66 | 67 | ## The base URL for the notebook server. 68 | # 69 | # Leading and trailing slashes can be omitted, and will automatically be added. 70 | #c.NotebookApp.base_url = '/' 71 | 72 | ## Specify what command to use to invoke a web browser when opening the notebook. 73 | # If not specified, the default browser will be determined by the `webbrowser` 74 | # standard library module, which allows setting of the BROWSER environment 75 | # variable to override it. 76 | #c.NotebookApp.browser = u'' 77 | 78 | ## The full path to an SSL/TLS certificate file. 79 | #c.NotebookApp.certfile = u'' 80 | 81 | ## The full path to a certificate authority certificate for SSL/TLS client 82 | # authentication. 83 | #c.NotebookApp.client_ca = u'' 84 | 85 | ## The config manager class to use 86 | #c.NotebookApp.config_manager_class = 'notebook.services.config.manager.ConfigManager' 87 | 88 | ## The notebook manager class to use. 89 | #c.NotebookApp.contents_manager_class = 'notebook.services.contents.filemanager.FileContentsManager' 90 | 91 | ## Extra keyword arguments to pass to `set_secure_cookie`. See tornado's 92 | # set_secure_cookie docs for details. 93 | #c.NotebookApp.cookie_options = {} 94 | 95 | ## The random bytes used to secure cookies. By default this is a new random 96 | # number every time you start the Notebook. Set it to a value in a config file 97 | # to enable logins to persist across server sessions. 98 | # 99 | # Note: Cookie secrets should be kept private, do not share config files with 100 | # cookie_secret stored in plaintext (you can read the value from a file). 101 | #c.NotebookApp.cookie_secret = '' 102 | 103 | ## The file where the cookie secret is stored. 104 | #c.NotebookApp.cookie_secret_file = u'' 105 | 106 | ## The default URL to redirect to from `/` 107 | #c.NotebookApp.default_url = '/tree' 108 | 109 | ## Disable cross-site-request-forgery protection 110 | # 111 | # Jupyter notebook 4.3.1 introduces protection from cross-site request 112 | # forgeries, requiring API requests to either: 113 | # 114 | # - originate from pages served by this server (validated with XSRF cookie and 115 | # token), or - authenticate with a token 116 | # 117 | # Some anonymous compute resources still desire the ability to run code, 118 | # completely without authentication. These services can disable all 119 | # authentication and security checks, with the full knowledge of what that 120 | # implies. 121 | #c.NotebookApp.disable_check_xsrf = False 122 | 123 | ## Whether to enable MathJax for typesetting math/TeX 124 | # 125 | # MathJax is the javascript library Jupyter uses to render math/LaTeX. It is 126 | # very large, so you may want to disable it if you have a slow internet 127 | # connection, or for offline use of the notebook. 128 | # 129 | # When disabled, equations etc. will appear as their untransformed TeX source. 130 | #c.NotebookApp.enable_mathjax = True 131 | 132 | ## extra paths to look for Javascript notebook extensions 133 | #c.NotebookApp.extra_nbextensions_path = [] 134 | 135 | ## Extra paths to search for serving static files. 136 | # 137 | # This allows adding javascript/css to be available from the notebook server 138 | # machine, or overriding individual files in the IPython 139 | #c.NotebookApp.extra_static_paths = [] 140 | 141 | ## Extra paths to search for serving jinja templates. 142 | # 143 | # Can be used to override templates from notebook.templates. 144 | #c.NotebookApp.extra_template_paths = [] 145 | 146 | ## 147 | #c.NotebookApp.file_to_run = '' 148 | 149 | ## Use minified JS file or not, mainly use during dev to avoid JS recompilation 150 | #c.NotebookApp.ignore_minified_js = False 151 | 152 | ## (bytes/sec) Maximum rate at which messages can be sent on iopub before they 153 | # are limited. 154 | #c.NotebookApp.iopub_data_rate_limit = 0 155 | 156 | ## (msg/sec) Maximum rate at which messages can be sent on iopub before they are 157 | # limited. 158 | #c.NotebookApp.iopub_msg_rate_limit = 0 159 | 160 | ## The IP address the notebook server will listen on. 161 | c.NotebookApp.ip = '0.0.0.0' 162 | 163 | ## Supply extra arguments that will be passed to Jinja environment. 164 | #c.NotebookApp.jinja_environment_options = {} 165 | 166 | ## Extra variables to supply to jinja templates when rendering. 167 | #c.NotebookApp.jinja_template_vars = {} 168 | 169 | ## The kernel manager class to use. 170 | #c.NotebookApp.kernel_manager_class = 'notebook.services.kernels.kernelmanager.MappingKernelManager' 171 | 172 | ## The kernel spec manager class to use. Should be a subclass of 173 | # `jupyter_client.kernelspec.KernelSpecManager`. 174 | # 175 | # The Api of KernelSpecManager is provisional and might change without warning 176 | # between this version of Jupyter and the next stable one. 177 | #c.NotebookApp.kernel_spec_manager_class = 'jupyter_client.kernelspec.KernelSpecManager' 178 | 179 | ## The full path to a private key file for usage with SSL/TLS. 180 | #c.NotebookApp.keyfile = u'' 181 | 182 | ## The login handler class to use. 183 | #c.NotebookApp.login_handler_class = 'notebook.auth.login.LoginHandler' 184 | 185 | ## The logout handler class to use. 186 | #c.NotebookApp.logout_handler_class = 'notebook.auth.logout.LogoutHandler' 187 | 188 | ## A custom url for MathJax.js. Should be in the form of a case-sensitive url to 189 | # MathJax, for example: /static/components/MathJax/MathJax.js 190 | #c.NotebookApp.mathjax_url = '' 191 | 192 | ## Dict of Python modules to load as notebook server extensions.Entry values can 193 | # be used to enable and disable the loading ofthe extensions. The extensions 194 | # will be loaded in alphabetical order. 195 | #c.NotebookApp.nbserver_extensions = {} 196 | 197 | ## The directory to use for notebooks and kernels. 198 | #c.NotebookApp.notebook_dir = u'' 199 | 200 | ## Whether to open in a browser after starting. The specific browser used is 201 | # platform dependent and determined by the python standard library `webbrowser` 202 | # module, unless it is overridden using the --browser (NotebookApp.browser) 203 | # configuration option. 204 | #c.NotebookApp.open_browser = True 205 | 206 | ## Hashed password to use for web authentication. 207 | # 208 | # To generate, type in a python/IPython shell: 209 | # 210 | # from notebook.auth import passwd; passwd() 211 | # 212 | # The string should be of the form type:salt:hashed-password. 213 | c.NotebookApp.password = u'sha1:dab3f87b6e86:4f8c054221008ed851ff491f307adb8a2a7f868f' 214 | 215 | ## The port the notebook server will listen on. 216 | #c.NotebookApp.port = 8888 217 | 218 | ## The number of additional ports to try if the specified port is not available. 219 | #c.NotebookApp.port_retries = 50 220 | 221 | ## DISABLED: use %pylab or %matplotlib in the notebook to enable matplotlib. 222 | #c.NotebookApp.pylab = 'disabled' 223 | 224 | ## (sec) Time window used to check the message and data rate limits. 225 | #c.NotebookApp.rate_limit_window = 1.0 226 | 227 | ## Reraise exceptions encountered loading server extensions? 228 | #c.NotebookApp.reraise_server_extension_failures = False 229 | 230 | ## DEPRECATED use the nbserver_extensions dict instead 231 | #c.NotebookApp.server_extensions = [] 232 | 233 | ## The session manager class to use. 234 | #c.NotebookApp.session_manager_class = 'notebook.services.sessions.sessionmanager.SessionManager' 235 | 236 | ## Supply SSL options for the tornado HTTPServer. See the tornado docs for 237 | # details. 238 | #c.NotebookApp.ssl_options = {} 239 | 240 | ## Token used for authenticating first-time connections to the server. 241 | # 242 | # When no password is enabled, the default is to generate a new, random token. 243 | # 244 | # Setting to an empty string disables authentication altogether, which is NOT 245 | # RECOMMENDED. 246 | c.NotebookApp.token = '' 247 | 248 | ## Supply overrides for the tornado.web.Application that the Jupyter notebook 249 | # uses. 250 | #c.NotebookApp.tornado_settings = {} 251 | 252 | ## Whether to trust or not X-Scheme/X-Forwarded-Proto and X-Real-Ip/X-Forwarded- 253 | # For headerssent by the upstream reverse proxy. Necessary if the proxy handles 254 | # SSL 255 | #c.NotebookApp.trust_xheaders = False 256 | 257 | ## DEPRECATED, use tornado_settings 258 | #c.NotebookApp.webapp_settings = {} 259 | 260 | ## The base URL for websockets, if it differs from the HTTP server (hint: it 261 | # almost certainly doesn't). 262 | # 263 | # Should be in the form of an HTTP origin: ws[s]://hostname[:port] 264 | #c.NotebookApp.websocket_url = '' 265 | 266 | #------------------------------------------------------------------------------ 267 | # ConnectionFileMixin(LoggingConfigurable) configuration 268 | #------------------------------------------------------------------------------ 269 | 270 | ## Mixin for configurable classes that work with connection files 271 | 272 | ## JSON file in which to store connection info [default: kernel-.json] 273 | # 274 | # This file will contain the IP, ports, and authentication key needed to connect 275 | # clients to this kernel. By default, this file will be created in the security 276 | # dir of the current profile, but can be specified by absolute path. 277 | #c.ConnectionFileMixin.connection_file = '' 278 | 279 | ## set the control (ROUTER) port [default: random] 280 | #c.ConnectionFileMixin.control_port = 0 281 | 282 | ## set the heartbeat port [default: random] 283 | #c.ConnectionFileMixin.hb_port = 0 284 | 285 | ## set the iopub (PUB) port [default: random] 286 | #c.ConnectionFileMixin.iopub_port = 0 287 | 288 | ## Set the kernel's IP address [default localhost]. If the IP address is 289 | # something other than localhost, then Consoles on other machines will be able 290 | # to connect to the Kernel, so be careful! 291 | #c.ConnectionFileMixin.ip = u'' 292 | 293 | ## set the shell (ROUTER) port [default: random] 294 | #c.ConnectionFileMixin.shell_port = 0 295 | 296 | ## set the stdin (ROUTER) port [default: random] 297 | #c.ConnectionFileMixin.stdin_port = 0 298 | 299 | ## 300 | #c.ConnectionFileMixin.transport = 'tcp' 301 | 302 | #------------------------------------------------------------------------------ 303 | # KernelManager(ConnectionFileMixin) configuration 304 | #------------------------------------------------------------------------------ 305 | 306 | ## Manages a single kernel in a subprocess on this host. 307 | # 308 | # This version starts kernels with Popen. 309 | 310 | ## Should we autorestart the kernel if it dies. 311 | #c.KernelManager.autorestart = True 312 | 313 | ## DEPRECATED: Use kernel_name instead. 314 | # 315 | # The Popen Command to launch the kernel. Override this if you have a custom 316 | # kernel. If kernel_cmd is specified in a configuration file, Jupyter does not 317 | # pass any arguments to the kernel, because it cannot make any assumptions about 318 | # the arguments that the kernel understands. In particular, this means that the 319 | # kernel does not receive the option --debug if it given on the Jupyter command 320 | # line. 321 | #c.KernelManager.kernel_cmd = [] 322 | 323 | ## Time to wait for a kernel to terminate before killing it, in seconds. 324 | #c.KernelManager.shutdown_wait_time = 5.0 325 | 326 | #------------------------------------------------------------------------------ 327 | # Session(Configurable) configuration 328 | #------------------------------------------------------------------------------ 329 | 330 | ## Object for handling serialization and sending of messages. 331 | # 332 | # The Session object handles building messages and sending them with ZMQ sockets 333 | # or ZMQStream objects. Objects can communicate with each other over the 334 | # network via Session objects, and only need to work with the dict-based IPython 335 | # message spec. The Session will handle serialization/deserialization, security, 336 | # and metadata. 337 | # 338 | # Sessions support configurable serialization via packer/unpacker traits, and 339 | # signing with HMAC digests via the key/keyfile traits. 340 | # 341 | # Parameters ---------- 342 | # 343 | # debug : bool 344 | # whether to trigger extra debugging statements 345 | # packer/unpacker : str : 'json', 'pickle' or import_string 346 | # importstrings for methods to serialize message parts. If just 347 | # 'json' or 'pickle', predefined JSON and pickle packers will be used. 348 | # Otherwise, the entire importstring must be used. 349 | # 350 | # The functions must accept at least valid JSON input, and output *bytes*. 351 | # 352 | # For example, to use msgpack: 353 | # packer = 'msgpack.packb', unpacker='msgpack.unpackb' 354 | # pack/unpack : callables 355 | # You can also set the pack/unpack callables for serialization directly. 356 | # session : bytes 357 | # the ID of this Session object. The default is to generate a new UUID. 358 | # username : unicode 359 | # username added to message headers. The default is to ask the OS. 360 | # key : bytes 361 | # The key used to initialize an HMAC signature. If unset, messages 362 | # will not be signed or checked. 363 | # keyfile : filepath 364 | # The file containing a key. If this is set, `key` will be initialized 365 | # to the contents of the file. 366 | 367 | ## Threshold (in bytes) beyond which an object's buffer should be extracted to 368 | # avoid pickling. 369 | #c.Session.buffer_threshold = 1024 370 | 371 | ## Whether to check PID to protect against calls after fork. 372 | # 373 | # This check can be disabled if fork-safety is handled elsewhere. 374 | #c.Session.check_pid = True 375 | 376 | ## Threshold (in bytes) beyond which a buffer should be sent without copying. 377 | #c.Session.copy_threshold = 65536 378 | 379 | ## Debug output in the Session 380 | #c.Session.debug = False 381 | 382 | ## The maximum number of digests to remember. 383 | # 384 | # The digest history will be culled when it exceeds this value. 385 | #c.Session.digest_history_size = 65536 386 | 387 | ## The maximum number of items for a container to be introspected for custom 388 | # serialization. Containers larger than this are pickled outright. 389 | #c.Session.item_threshold = 64 390 | 391 | ## execution key, for signing messages. 392 | #c.Session.key = '' 393 | 394 | ## path to file containing execution key. 395 | #c.Session.keyfile = '' 396 | 397 | ## Metadata dictionary, which serves as the default top-level metadata dict for 398 | # each message. 399 | #c.Session.metadata = {} 400 | 401 | ## The name of the packer for serializing messages. Should be one of 'json', 402 | # 'pickle', or an import name for a custom callable serializer. 403 | #c.Session.packer = 'json' 404 | 405 | ## The UUID identifying this session. 406 | #c.Session.session = u'' 407 | 408 | ## The digest scheme used to construct the message signatures. Must have the form 409 | # 'hmac-HASH'. 410 | #c.Session.signature_scheme = 'hmac-sha256' 411 | 412 | ## The name of the unpacker for unserializing messages. Only used with custom 413 | # functions for `packer`. 414 | #c.Session.unpacker = 'json' 415 | 416 | ## Username for the Session. Default is your system username. 417 | #c.Session.username = u'username' 418 | 419 | #------------------------------------------------------------------------------ 420 | # MultiKernelManager(LoggingConfigurable) configuration 421 | #------------------------------------------------------------------------------ 422 | 423 | ## A class for managing multiple kernels. 424 | 425 | ## The name of the default kernel to start 426 | #c.MultiKernelManager.default_kernel_name = 'python2' 427 | 428 | ## The kernel manager class. This is configurable to allow subclassing of the 429 | # KernelManager for customized behavior. 430 | #c.MultiKernelManager.kernel_manager_class = 'jupyter_client.ioloop.IOLoopKernelManager' 431 | 432 | #------------------------------------------------------------------------------ 433 | # MappingKernelManager(MultiKernelManager) configuration 434 | #------------------------------------------------------------------------------ 435 | 436 | ## A KernelManager that handles notebook mapping and HTTP error handling 437 | 438 | ## 439 | #c.MappingKernelManager.root_dir = u'' 440 | 441 | #------------------------------------------------------------------------------ 442 | # ContentsManager(LoggingConfigurable) configuration 443 | #------------------------------------------------------------------------------ 444 | 445 | ## Base class for serving files and directories. 446 | # 447 | # This serves any text or binary file, as well as directories, with special 448 | # handling for JSON notebook documents. 449 | # 450 | # Most APIs take a path argument, which is always an API-style unicode path, and 451 | # always refers to a directory. 452 | # 453 | # - unicode, not url-escaped 454 | # - '/'-separated 455 | # - leading and trailing '/' will be stripped 456 | # - if unspecified, path defaults to '', 457 | # indicating the root path. 458 | 459 | ## 460 | #c.ContentsManager.checkpoints = None 461 | 462 | ## 463 | #c.ContentsManager.checkpoints_class = 'notebook.services.contents.checkpoints.Checkpoints' 464 | 465 | ## 466 | #c.ContentsManager.checkpoints_kwargs = {} 467 | 468 | ## Glob patterns to hide in file and directory listings. 469 | #c.ContentsManager.hide_globs = [u'__pycache__', '*.pyc', '*.pyo', '.DS_Store', '*.so', '*.dylib', '*~'] 470 | 471 | ## Python callable or importstring thereof 472 | # 473 | # To be called on a contents model prior to save. 474 | # 475 | # This can be used to process the structure, such as removing notebook outputs 476 | # or other side effects that should not be saved. 477 | # 478 | # It will be called as (all arguments passed by keyword):: 479 | # 480 | # hook(path=path, model=model, contents_manager=self) 481 | # 482 | # - model: the model to be saved. Includes file contents. 483 | # Modifying this dict will affect the file that is stored. 484 | # - path: the API path of the save destination 485 | # - contents_manager: this ContentsManager instance 486 | #c.ContentsManager.pre_save_hook = None 487 | 488 | ## The base name used when creating untitled directories. 489 | #c.ContentsManager.untitled_directory = 'Untitled Folder' 490 | 491 | ## The base name used when creating untitled files. 492 | #c.ContentsManager.untitled_file = 'untitled' 493 | 494 | ## The base name used when creating untitled notebooks. 495 | #c.ContentsManager.untitled_notebook = 'Untitled' 496 | 497 | #------------------------------------------------------------------------------ 498 | # FileManagerMixin(Configurable) configuration 499 | #------------------------------------------------------------------------------ 500 | 501 | ## Mixin for ContentsAPI classes that interact with the filesystem. 502 | # 503 | # Provides facilities for reading, writing, and copying both notebooks and 504 | # generic files. 505 | # 506 | # Shared by FileContentsManager and FileCheckpoints. 507 | # 508 | # Note ---- Classes using this mixin must provide the following attributes: 509 | # 510 | # root_dir : unicode 511 | # A directory against against which API-style paths are to be resolved. 512 | # 513 | # log : logging.Logger 514 | 515 | ## By default notebooks are saved on disk on a temporary file and then if 516 | # succefully written, it replaces the old ones. This procedure, namely 517 | # 'atomic_writing', causes some bugs on file system whitout operation order 518 | # enforcement (like some networked fs). If set to False, the new notebook is 519 | # written directly on the old one which could fail (eg: full filesystem or quota 520 | # ) 521 | #c.FileManagerMixin.use_atomic_writing = True 522 | 523 | #------------------------------------------------------------------------------ 524 | # FileContentsManager(FileManagerMixin,ContentsManager) configuration 525 | #------------------------------------------------------------------------------ 526 | 527 | ## Python callable or importstring thereof 528 | # 529 | # to be called on the path of a file just saved. 530 | # 531 | # This can be used to process the file on disk, such as converting the notebook 532 | # to a script or HTML via nbconvert. 533 | # 534 | # It will be called as (all arguments passed by keyword):: 535 | # 536 | # hook(os_path=os_path, model=model, contents_manager=instance) 537 | # 538 | # - path: the filesystem path to the file just written - model: the model 539 | # representing the file - contents_manager: this ContentsManager instance 540 | #c.FileContentsManager.post_save_hook = None 541 | 542 | ## 543 | #c.FileContentsManager.root_dir = u'' 544 | 545 | ## DEPRECATED, use post_save_hook. Will be removed in Notebook 5.0 546 | #c.FileContentsManager.save_script = False 547 | 548 | #------------------------------------------------------------------------------ 549 | # NotebookNotary(LoggingConfigurable) configuration 550 | #------------------------------------------------------------------------------ 551 | 552 | ## A class for computing and verifying notebook signatures. 553 | 554 | ## The hashing algorithm used to sign notebooks. 555 | #c.NotebookNotary.algorithm = 'sha256' 556 | 557 | ## The sqlite file in which to store notebook signatures. By default, this will 558 | # be in your Jupyter data directory. You can set it to ':memory:' to disable 559 | # sqlite writing to the filesystem. 560 | #c.NotebookNotary.db_file = u'' 561 | 562 | ## The secret key with which notebooks are signed. 563 | #c.NotebookNotary.secret = '' 564 | 565 | ## The file where the secret key is stored. 566 | #c.NotebookNotary.secret_file = u'' 567 | 568 | ## A callable returning the storage backend for notebook signatures. The default 569 | # uses an SQLite database. 570 | #c.NotebookNotary.store_factory = traitlets.Undefined 571 | 572 | #------------------------------------------------------------------------------ 573 | # KernelSpecManager(LoggingConfigurable) configuration 574 | #------------------------------------------------------------------------------ 575 | 576 | ## If there is no Python kernelspec registered and the IPython kernel is 577 | # available, ensure it is added to the spec list. 578 | #c.KernelSpecManager.ensure_native_kernel = True 579 | 580 | ## The kernel spec class. This is configurable to allow subclassing of the 581 | # KernelSpecManager for customized behavior. 582 | #c.KernelSpecManager.kernel_spec_class = 'jupyter_client.kernelspec.KernelSpec' 583 | 584 | ## Whitelist of allowed kernel names. 585 | # 586 | # By default, all installed kernels are allowed. 587 | #c.KernelSpecManager.whitelist = set([]) 588 | -------------------------------------------------------------------------------- /repository.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: gazebo_ros_pkgs 3 | uri: 'https://github.com/ros-simulation/gazebo_ros_pkgs.git' 4 | version: indigo-devel 5 | 6 | - git: 7 | local-name: pysdf 8 | uri: https://github.com/shadow-robot/pysdf.git 9 | 10 | - git: 11 | local-name: gazebo2rviz 12 | version: F_add_moveit_funtionallity 13 | uri: https://github.com/shadow-robot/gazebo2rviz.git 14 | 15 | - git: 16 | local-name: universal_robot 17 | version: F_gazebo_7_docker 18 | uri: https://github.com/shadow-robot/universal_robot.git 19 | -------------------------------------------------------------------------------- /setup_gzweb.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | nohup Xvfb :1 -screen 0 1024x768x16 &> xvfb.log & 3 | DISPLAY=:1.0 4 | export DISPLAY 5 | 6 | cd /root 7 | hg clone -r gzweb_1.3.0 https://bitbucket.org/osrf/gzweb 8 | source "/workspace/devel/setup.bash" 9 | source "/usr/share/gazebo/setup.sh" 10 | cd /root 11 | wget -l 2 -nc -r "http://models.gazebosim.org/" --accept gz 12 | cd "models.gazebosim.org" 13 | for i in * ; do tar -zvxf "$i/model.tar.gz" ; done 14 | mkdir -p $HOME/.gazebo/models 15 | cp -vfR * "$HOME/.gazebo/models/" 16 | cd /root/gzweb 17 | GAZEBO_MODEL_PATH=/workspace/src:/workspace/src/universal_robot:~/.gazebo/models:${GAZEBO_MODEL_PATH} ./deploy.sh -m 18 | -------------------------------------------------------------------------------- /shippable.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | python: 3 | - 2.7 4 | 5 | # codecov token and slack web hook URL (encrypted) 6 | env: 7 | - secure: EqEpHqAismOsNFDqTqkCRKoapJDOFms+IaYoyQaNfk1QRU72NVAP3kpQS2UXWSwfizZtKInw332eh+0fanPr/Xdozp9xDKwAXx5ugj3znP7ffC1F15r79QvL+wpelfmkI0xThUBnkvqR+6x4DKqoPNh0C9GfF8RRvlv1xhrrNYZKHfq8n8/YOE2MBCnnF5y3oX1+qOwuns4vqRomgM1jUhPBIhAd7SQgDAG0QNLFfHZGOe1Dlo2nS3BgQDFrIeqIWDyjfXqxMTfbV6e+sqtiKp5uphkyxWg5HGw7O+AhQy7FD1Pe4UVd6xZ65S+PCwWBg0Y66tQuYzfFXUCGOU4/IA== 8 | 9 | build: 10 | pre_ci_boot: 11 | image_name: shadowrobot/ubuntu-ros-indigo-build-tools 12 | image_tag: latest 13 | pull: true 14 | 15 | options: "-e HOME=/root" 16 | 17 | ci: 18 | - sudo apt-get update 19 | - sudo apt-get install curl -y 20 | - export toolset_branch="master" 21 | - export server_type="shippable" 22 | - export used_modules="build_pr_only,check_cache,code_coverage" 23 | - export remote_shell_script="https://raw.githubusercontent.com/shadow-robot/sr-build-tools/$toolset_branch/bin/sr-run-ci-build.sh" 24 | - curl -s "$( echo "$remote_shell_script" | sed 's/#/%23/g' )" | bash /dev/stdin "$toolset_branch" $server_type $used_modules 25 | on_failure: 26 | - curl -X POST --data-urlencode "payload={\"channel\":\"#bots\", \"username\":\"shippable\", \"text\":\"[$SHIPPABLE_REPO_SLUG branch $BRANCH] <$BUILD_URL|Build $SHIPPABLE_BUILD_NUMBER> failed\", \"icon_emoji\":\":monkey_face:\"}" $SLACK_WEB_HOOK_URL 27 | 28 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: fh_desc 4 | relative_path: smart_grasping_sandbox.urdf 5 | SRDF: 6 | relative_path: config/ur10.srdf 7 | CONFIG: 8 | generated_timestamp: 1480683875 -------------------------------------------------------------------------------- /smart_grasp_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smart_grasp_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - shoulder_pan_joint 5 | - shoulder_lift_joint 6 | - elbow_joint 7 | - wrist_1_joint 8 | - wrist_2_joint 9 | - wrist_3_joint 10 | - name: fake_hand_controller 11 | joints: 12 | - H1_F1J1 13 | - H1_F1J2 14 | - H1_F1J3 15 | - H1_F2J1 16 | - H1_F2J2 17 | - H1_F2J3 18 | - H1_F3J1 19 | - H1_F3J2 20 | - H1_F3J3 -------------------------------------------------------------------------------- /smart_grasp_moveit_config/config/gazebo_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: arm_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | joints: 6 | - shoulder_pan_joint 7 | - shoulder_lift_joint 8 | - elbow_joint 9 | - wrist_1_joint 10 | - wrist_2_joint 11 | - wrist_3_joint 12 | - name: hand_controller 13 | action_ns: follow_joint_trajectory 14 | type: FollowJointTrajectory 15 | joints: 16 | - H1_F1J1 17 | - H1_F1J2 18 | - H1_F1J3 19 | - H1_F2J1 20 | - H1_F2J2 21 | - H1_F2J3 22 | - H1_F3J1 23 | - H1_F3J2 24 | - H1_F3J3 -------------------------------------------------------------------------------- /smart_grasp_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | H1_F1J1: 6 | has_velocity_limits: true 7 | max_velocity: 1 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | H1_F1J2: 11 | has_velocity_limits: true 12 | max_velocity: 1 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | H1_F1J3: 16 | has_velocity_limits: true 17 | max_velocity: 1 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | H1_F2J1: 21 | has_velocity_limits: true 22 | max_velocity: 1 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | H1_F2J2: 26 | has_velocity_limits: true 27 | max_velocity: 1 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | H1_F2J3: 31 | has_velocity_limits: true 32 | max_velocity: 1 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | H1_F3J1: 36 | has_velocity_limits: true 37 | max_velocity: 1 38 | has_acceleration_limits: false 39 | max_acceleration: 0 40 | H1_F3J2: 41 | has_velocity_limits: true 42 | max_velocity: 1 43 | has_acceleration_limits: false 44 | max_acceleration: 0 45 | H1_F3J3: 46 | has_velocity_limits: true 47 | max_velocity: 1 48 | has_acceleration_limits: false 49 | max_acceleration: 0 50 | elbow_joint: 51 | has_velocity_limits: true 52 | max_velocity: 3.15 53 | has_acceleration_limits: false 54 | max_acceleration: 0 55 | shoulder_lift_joint: 56 | has_velocity_limits: true 57 | max_velocity: 2.16 58 | has_acceleration_limits: false 59 | max_acceleration: 0 60 | shoulder_pan_joint: 61 | has_velocity_limits: true 62 | max_velocity: 2.16 63 | has_acceleration_limits: false 64 | max_acceleration: 0 65 | wrist_1_joint: 66 | has_velocity_limits: true 67 | max_velocity: 3.2 68 | has_acceleration_limits: false 69 | max_acceleration: 0 70 | wrist_2_joint: 71 | has_velocity_limits: true 72 | max_velocity: 3.2 73 | has_acceleration_limits: false 74 | max_acceleration: 0 75 | wrist_3_joint: 76 | has_velocity_limits: true 77 | max_velocity: 3.2 78 | has_acceleration_limits: false 79 | max_acceleration: 0 -------------------------------------------------------------------------------- /smart_grasp_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /smart_grasp_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | arm: 56 | planner_configs: 57 | - SBLkConfigDefault 58 | - ESTkConfigDefault 59 | - LBKPIECEkConfigDefault 60 | - BKPIECEkConfigDefault 61 | - KPIECEkConfigDefault 62 | - RRTkConfigDefault 63 | - RRTConnectkConfigDefault 64 | - RRTstarkConfigDefault 65 | - TRRTkConfigDefault 66 | - PRMkConfigDefault 67 | - PRMstarkConfigDefault 68 | #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) 69 | longest_valid_segment_fraction: 0.05 70 | default_planner_config: RRTkConfigDefault 71 | hand: 72 | planner_configs: 73 | - SBLkConfigDefault 74 | - ESTkConfigDefault 75 | - LBKPIECEkConfigDefault 76 | - BKPIECEkConfigDefault 77 | - KPIECEkConfigDefault 78 | - RRTkConfigDefault 79 | - RRTConnectkConfigDefault 80 | - RRTstarkConfigDefault 81 | - TRRTkConfigDefault 82 | - PRMkConfigDefault 83 | - PRMstarkConfigDefault 84 | longest_valid_segment_fraction: 0.05 85 | default_planner_config: RRTkConfigDefault 86 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/config/ur10.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | [/move_group/fake_controller_joint_states] 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /MotionPlanning1 9 | Splitter Ratio: 0.5 10 | Tree Height: 341 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: moveit_rviz_plugin/MotionPlanning 52 | Enabled: true 53 | Move Group Namespace: "" 54 | MoveIt_Goal_Tolerance: 0 55 | MoveIt_Planning_Attempts: 10 56 | MoveIt_Planning_Time: 5 57 | MoveIt_Use_Constraint_Aware_IK: true 58 | MoveIt_Warehouse_Host: 127.0.0.1 59 | MoveIt_Warehouse_Port: 33829 60 | MoveIt_Workspace: 61 | Center: 62 | X: 0 63 | Y: 0 64 | Z: 0 65 | Size: 66 | X: 2 67 | Y: 2 68 | Z: 2 69 | Name: MotionPlanning 70 | Planned Path: 71 | Interrupt Display: false 72 | Links: 73 | All Links Enabled: true 74 | Expand Joint Details: false 75 | Expand Link Details: false 76 | Expand Tree: false 77 | H1_F1_base_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | H1_F1_link_1: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | H1_F1_link_2: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | H1_F1_optoforce: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | H1_F1_tip: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | H1_F2_base_link: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | H1_F2_link_1: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | H1_F2_link_2: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | H1_F2_optoforce: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | H1_F2_tip: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | H1_F3_base_link: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | H1_F3_link_1: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | H1_F3_link_2: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | Value: true 140 | H1_F3_optoforce: 141 | Alpha: 1 142 | Show Axes: false 143 | Show Trail: false 144 | H1_F3_tip: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | H1_WR_face_plate: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | H1_WR_force_torque_sensor: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | H1_WR_wrist: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | H1_WR_wrist_base: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | H1_base_attach: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | H1_base_link: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | Value: true 177 | H1_insert_F1: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | H1_insert_F2: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | H1_insert_F3: 186 | Alpha: 1 187 | Show Axes: false 188 | Show Trail: false 189 | H1_wrist_attach: 190 | Alpha: 1 191 | Show Axes: false 192 | Show Trail: false 193 | Link Tree Style: Links in Alphabetic Order 194 | base: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | base_link: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | ee_link: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | forearm_link: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | Value: true 213 | shoulder_link: 214 | Alpha: 1 215 | Show Axes: false 216 | Show Trail: false 217 | Value: true 218 | tool0: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | upper_arm_link: 223 | Alpha: 1 224 | Show Axes: false 225 | Show Trail: false 226 | Value: true 227 | world: 228 | Alpha: 1 229 | Show Axes: false 230 | Show Trail: false 231 | wrist_1_link: 232 | Alpha: 1 233 | Show Axes: false 234 | Show Trail: false 235 | Value: true 236 | wrist_2_link: 237 | Alpha: 1 238 | Show Axes: false 239 | Show Trail: false 240 | Value: true 241 | wrist_3_link: 242 | Alpha: 1 243 | Show Axes: false 244 | Show Trail: false 245 | Value: true 246 | Loop Animation: false 247 | Robot Alpha: 0.5 248 | Show Robot Collision: false 249 | Show Robot Visual: true 250 | Show Trail: false 251 | State Display Time: 0.05 s 252 | Trajectory Topic: /move_group/display_planned_path 253 | Planning Metrics: 254 | Payload: 1 255 | Show Joint Torques: false 256 | Show Manipulability: false 257 | Show Manipulability Index: false 258 | Show Weight Limit: false 259 | TextHeight: 0.08 260 | Planning Request: 261 | Colliding Link Color: 255; 0; 0 262 | Goal State Alpha: 1 263 | Goal State Color: 250; 128; 0 264 | Interactive Marker Size: 0 265 | Joint Violation Color: 255; 0; 255 266 | Planning Group: arm 267 | Query Goal State: true 268 | Query Start State: true 269 | Show Workspace: false 270 | Start State Alpha: 1 271 | Start State Color: 0; 255; 0 272 | Planning Scene Topic: /move_group/monitored_planning_scene 273 | Robot Description: robot_description 274 | Scene Geometry: 275 | Scene Alpha: 0.9 276 | Scene Color: 50; 230; 50 277 | Scene Display Time: 0.2 278 | Show Scene Geometry: true 279 | Voxel Coloring: Z-Axis 280 | Voxel Rendering: Occupied Voxels 281 | Scene Robot: 282 | Attached Body Color: 150; 50; 150 283 | Links: 284 | All Links Enabled: true 285 | Expand Joint Details: false 286 | Expand Link Details: false 287 | Expand Tree: false 288 | H1_F1_base_link: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | H1_F1_link_1: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | H1_F1_link_2: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | H1_F1_optoforce: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | H1_F1_tip: 308 | Alpha: 1 309 | Show Axes: false 310 | Show Trail: false 311 | Value: true 312 | H1_F2_base_link: 313 | Alpha: 1 314 | Show Axes: false 315 | Show Trail: false 316 | Value: true 317 | H1_F2_link_1: 318 | Alpha: 1 319 | Show Axes: false 320 | Show Trail: false 321 | Value: true 322 | H1_F2_link_2: 323 | Alpha: 1 324 | Show Axes: false 325 | Show Trail: false 326 | Value: true 327 | H1_F2_optoforce: 328 | Alpha: 1 329 | Show Axes: false 330 | Show Trail: false 331 | H1_F2_tip: 332 | Alpha: 1 333 | Show Axes: false 334 | Show Trail: false 335 | Value: true 336 | H1_F3_base_link: 337 | Alpha: 1 338 | Show Axes: false 339 | Show Trail: false 340 | Value: true 341 | H1_F3_link_1: 342 | Alpha: 1 343 | Show Axes: false 344 | Show Trail: false 345 | Value: true 346 | H1_F3_link_2: 347 | Alpha: 1 348 | Show Axes: false 349 | Show Trail: false 350 | Value: true 351 | H1_F3_optoforce: 352 | Alpha: 1 353 | Show Axes: false 354 | Show Trail: false 355 | H1_F3_tip: 356 | Alpha: 1 357 | Show Axes: false 358 | Show Trail: false 359 | Value: true 360 | H1_WR_face_plate: 361 | Alpha: 1 362 | Show Axes: false 363 | Show Trail: false 364 | H1_WR_force_torque_sensor: 365 | Alpha: 1 366 | Show Axes: false 367 | Show Trail: false 368 | Value: true 369 | H1_WR_wrist: 370 | Alpha: 1 371 | Show Axes: false 372 | Show Trail: false 373 | Value: true 374 | H1_WR_wrist_base: 375 | Alpha: 1 376 | Show Axes: false 377 | Show Trail: false 378 | Value: true 379 | H1_base_attach: 380 | Alpha: 1 381 | Show Axes: false 382 | Show Trail: false 383 | H1_base_link: 384 | Alpha: 1 385 | Show Axes: false 386 | Show Trail: false 387 | Value: true 388 | H1_insert_F1: 389 | Alpha: 1 390 | Show Axes: false 391 | Show Trail: false 392 | H1_insert_F2: 393 | Alpha: 1 394 | Show Axes: false 395 | Show Trail: false 396 | H1_insert_F3: 397 | Alpha: 1 398 | Show Axes: false 399 | Show Trail: false 400 | H1_wrist_attach: 401 | Alpha: 1 402 | Show Axes: false 403 | Show Trail: false 404 | Link Tree Style: Links in Alphabetic Order 405 | base: 406 | Alpha: 1 407 | Show Axes: false 408 | Show Trail: false 409 | base_link: 410 | Alpha: 1 411 | Show Axes: false 412 | Show Trail: false 413 | Value: true 414 | ee_link: 415 | Alpha: 1 416 | Show Axes: false 417 | Show Trail: false 418 | Value: true 419 | forearm_link: 420 | Alpha: 1 421 | Show Axes: false 422 | Show Trail: false 423 | Value: true 424 | shoulder_link: 425 | Alpha: 1 426 | Show Axes: false 427 | Show Trail: false 428 | Value: true 429 | tool0: 430 | Alpha: 1 431 | Show Axes: false 432 | Show Trail: false 433 | upper_arm_link: 434 | Alpha: 1 435 | Show Axes: false 436 | Show Trail: false 437 | Value: true 438 | world: 439 | Alpha: 1 440 | Show Axes: false 441 | Show Trail: false 442 | wrist_1_link: 443 | Alpha: 1 444 | Show Axes: false 445 | Show Trail: false 446 | Value: true 447 | wrist_2_link: 448 | Alpha: 1 449 | Show Axes: false 450 | Show Trail: false 451 | Value: true 452 | wrist_3_link: 453 | Alpha: 1 454 | Show Axes: false 455 | Show Trail: false 456 | Value: true 457 | Robot Alpha: 1 458 | Show Robot Collision: false 459 | Show Robot Visual: true 460 | Value: true 461 | Enabled: true 462 | Global Options: 463 | Background Color: 48; 48; 48 464 | Fixed Frame: world 465 | Frame Rate: 30 466 | Name: root 467 | Tools: 468 | - Class: rviz/Interact 469 | Hide Inactive Objects: true 470 | - Class: rviz/MoveCamera 471 | - Class: rviz/Select 472 | - Class: rviz/FocusCamera 473 | - Class: rviz/Measure 474 | - Class: rviz/SetInitialPose 475 | Topic: /initialpose 476 | - Class: rviz/SetGoal 477 | Topic: /move_base_simple/goal 478 | - Class: rviz/PublishPoint 479 | Single click: true 480 | Topic: /clicked_point 481 | Value: true 482 | Views: 483 | Current: 484 | Class: rviz/Orbit 485 | Distance: 2.12145 486 | Enable Stereo Rendering: 487 | Stereo Eye Separation: 0.06 488 | Stereo Focal Distance: 1 489 | Swap Stereo Eyes: false 490 | Value: false 491 | Focal Point: 492 | X: 0.0230437 493 | Y: -0.084909 494 | Z: 0.879697 495 | Name: Current View 496 | Near Clip Distance: 0.01 497 | Pitch: 0.804797 498 | Target Frame: 499 | Value: Orbit (rviz) 500 | Yaw: 1.25039 501 | Saved: ~ 502 | Window Geometry: 503 | Displays: 504 | collapsed: false 505 | Height: 1028 506 | Hide Left Dock: false 507 | Hide Right Dock: true 508 | Motion Planning: 509 | collapsed: false 510 | QMainWindow State: 000000ff00000000fd00000004000000000000038900000368fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000003600000368000001ad01000016fa000000010100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000014200fffffffb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000000000003890000025800ffffff000000010000011b00000366fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000040000003660000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000066e00000046fc0100000002fb0000000800540069006d006501000000000000066e0000024600fffffffb0000000800540069006d00650100000000000004500000000000000000000002df0000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 511 | Selection: 512 | collapsed: false 513 | Time: 514 | collapsed: false 515 | Tool Properties: 516 | collapsed: false 517 | Views: 518 | collapsed: true 519 | Width: 1646 520 | X: 22 521 | Y: 14 522 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/smart_grasping_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/ur10_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/ur10_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /smart_grasp_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smart_grasp_moveit_config 4 | 0.2.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the ur10 with the MoveIt Motion Planning Framework 7 | 8 | MoveIt Setup Assistant 9 | MoveIt Setup Assistant 10 | 11 | GPLv3 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit_setup_assistant/issues 15 | https://github.com/ros-planning/moveit_setup_assistant 16 | 17 | moveit_ros_move_group 18 | moveit_planners_ompl 19 | moveit_ros_visualization 20 | joint_state_publisher 21 | robot_state_publisher 22 | xacro 23 | fh_desc 24 | fh_desc 25 | 26 | 27 | catkin 28 | 29 | 30 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(smart_grasping_sandbox) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy 9 | fh_desc 10 | smart_grasp_moveit_config 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs # Or other packages containing msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES smart_grasping_sandbox 105 | # CATKIN_DEPENDS fh_desc smart_grasp_moveit_config 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(smart_grasping_sandbox 122 | # src/${PROJECT_NAME}/smart_grasping_sandbox.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(smart_grasping_sandbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(smart_grasping_sandbox_node src/smart_grasping_sandbox_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(smart_grasping_sandbox_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(smart_grasping_sandbox_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS smart_grasping_sandbox smart_grasping_sandbox_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_smart_grasping_sandbox.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/launch/smart_grasping_sandbox.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/building/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | building 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Software Team 10 | software@shadowrobot.com 11 | 12 | 13 | 14 | Building. 15 | 16 | 17 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/building/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 0.25 0.18 0 0 -0 0 4 | 5 | -1.42 -0 0 0 -0 1.5708 6 | 7 | -1.1775 0 1.25 0 -0 0 8 | 9 | 10 | 0.895 0.15 2.5 11 | 12 | 13 | 14 | 18 | 1 1 1 1 19 | 20 | 21 | 0 22 | 23 | 24 | 25 | 26 | 27 | 0.895 0.15 2.5 28 | 29 | 30 | -1.1775 0 1.25 0 -0 0 31 | 10 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 0.4475 0 0.25 0 -0 0 47 | 48 | 49 | 2.355 0.15 0.5 50 | 51 | 52 | 53 | 57 | 1 1 1 1 58 | 59 | 60 | 0 61 | 62 | 63 | 64 | 65 | 66 | 2.355 0.15 0.5 67 | 68 | 69 | 0.4475 0 0.25 0 -0 0 70 | 10 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 1.3175 0 1.5 0 -0 0 86 | 87 | 88 | 0.615 0.15 2 89 | 90 | 91 | 92 | 96 | 1 1 1 1 97 | 98 | 99 | 0 100 | 101 | 102 | 103 | 104 | 105 | 0.615 0.15 2 106 | 107 | 108 | 1.3175 0 1.5 0 -0 0 109 | 10 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 0.14 0 1.9 0 -0 0 125 | 126 | 127 | 1.74 0.15 1.2 128 | 129 | 130 | 131 | 135 | 1 1 1 1 136 | 137 | 138 | 0 139 | 140 | 141 | 142 | 143 | 144 | 1.74 0.15 1.2 145 | 146 | 147 | 0.14 0 1.9 0 -0 0 148 | 10 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 0 163 | 0 164 | 165 | 166 | 0.005 1.55 0 0 -0 0 167 | 168 | -1.27727 0 1.25 0 -0 0 169 | 170 | 171 | 0.425456 0.15 2.5 172 | 173 | 174 | 175 | 179 | 1 1 1 1 180 | 181 | 182 | 0 183 | 184 | 185 | 186 | 187 | 188 | 0.425456 0.15 2.5 189 | 190 | 191 | -1.27727 0 1.25 0 -0 0 192 | 10 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 0.212728 0 0.25 0 -0 0 208 | 209 | 210 | 2.55454 0.15 0.5 211 | 212 | 213 | 214 | 218 | 1 1 1 1 219 | 220 | 221 | 0 222 | 223 | 224 | 225 | 226 | 227 | 2.55454 0.15 0.5 228 | 229 | 230 | 0.212728 0 0.25 0 -0 0 231 | 10 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 1.21273 0 1.5 0 -0 0 247 | 248 | 249 | 0.554544 0.15 2 250 | 251 | 252 | 253 | 257 | 1 1 1 1 258 | 259 | 260 | 0 261 | 262 | 263 | 264 | 265 | 266 | 0.554544 0.15 2 267 | 268 | 269 | 1.21273 0 1.5 0 -0 0 270 | 10 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | -0.064544 0 1.9 0 -0 0 286 | 287 | 288 | 2 0.15 1.2 289 | 290 | 291 | 292 | 296 | 1 1 1 1 297 | 298 | 299 | 0 300 | 301 | 302 | 303 | 304 | 305 | 2 0.15 1.2 306 | 307 | 308 | -0.064544 0 1.9 0 -0 0 309 | 10 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 0 324 | 0 325 | 326 | 1 327 | 328 | 329 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/floor/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | floor 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Software Team 10 | software@shadowrobot.com 11 | 12 | 13 | 14 | Floor. 15 | 16 | 17 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/floor/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 0.0 0.0 -0.025 0 0 0 5 | 6 | 7 | 1 8 | 9 | 0.166667 10 | 0 11 | 0 12 | 0.166667 13 | 0 14 | 0.166667 15 | 16 | 17 | 18 | 19 | 20 | 4.0 4.0 0.05 21 | 22 | 23 | 10 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 4.0 4.0 0.05 41 | 42 | 43 | 44 | 48 | 1 1 1 1 49 | 50 | 51 | 0 52 | 0 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_pilar/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | kinect_pilar 6 | 1.0 7 | model.sdf 8 | 9 | 10 | Software Team 11 | software@shadowrobot.com 12 | 13 | 14 | 15 | Kinect pilar 16 | 17 | 18 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_pilar/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 1.39 0.242 0.5 0 -0 0 5 | 6 | 7 | 1 8 | 9 | 0.145833 10 | 0 11 | 0 12 | 0.145833 13 | 0 14 | 0.125 15 | 16 | 17 | 18 | 19 | 20 | 0.04 21 | 1 22 | 23 | 24 | 10 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 0.04 42 | 1 43 | 44 | 45 | 46 | 50 | 1 1 1 1 51 | 52 | 53 | 0 54 | 0 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/materials/textures/kinect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/smart_grasping_sandbox/models/kinect_ros/materials/textures/kinect.png -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/model-1_2.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.036 0 0 0 5 | 6 | 7 | 0.1 8 | 9 | 10 | 11 | 12 | 13 | 0.073000 0.276000 0.072000 14 | 15 | 16 | 17 | 18 | 19 | 20 | model://kinect/meshes/kinect.dae 21 | 22 | 23 | 24 | 25 | 26 | 20 27 | 28 | 1.047198 29 | 30 | 640 31 | 480 32 | R8G8B8 33 | 34 | 35 | 0.05 36 | 3 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.036 0 0 0 5 | 6 | 7 | 0.1 8 | 9 | 10 | 11 | 12 | 13 | 0.073000 0.276000 0.072000 14 | 15 | 16 | 17 | 18 | 19 | 20 | model://kinect/meshes/kinect.dae 21 | 22 | 23 | 24 | 25 | 20 26 | 27 | 1.047198 28 | 29 | 640 30 | 480 31 | R8G8B8 32 | 33 | 34 | 0.05 35 | 3 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.036 0 0 0 5 | 6 | 7 | 0.1 8 | 9 | 10 | 11 | 12 | 13 | 0.073000 0.276000 0.072000 14 | 15 | 16 | 17 | 18 | 19 | 20 | model://kinect/meshes/kinect.dae 21 | 22 | 23 | 24 | 25 | 20 26 | 27 | 1.047198 28 | 29 | 640 30 | 480 31 | R8G8B8 32 | 33 | 34 | 0.05 35 | 3 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1.0 5 | kinect_ros 6 | model-1_2.sdf 7 | model-1_3.sdf 8 | model-1_4.sdf 9 | model.sdf 10 | 11 | 12 | Nate Koenig 13 | nate@osrfoundation.org 14 | 15 | 16 | 17 | A model of the kinect sensor 18 | 19 | 20 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0.036 0 0 0 5 | 6 | 7 | 0.1 8 | 9 | 10 | 11 | 12 | 0.073000 0.276000 0.072000 13 | 14 | 15 | 16 | 17 | 18 | 19 | model://kinect/meshes/kinect.dae 20 | 21 | 22 | 23 | 24 | 20 25 | 26 | 1.047198 27 | 28 | 640 29 | 480 30 | R8G8B8 31 | 32 | 33 | 0.05 34 | 3 35 | 36 | 37 | 38 | 0.2 39 | true 40 | 42 | 0.0 43 | camera_ir 44 | /camera/depth/image_raw 45 | /camera/depth/camera_info 46 | /camera/depth/image_raw 47 | /camera/depth/camera_info 48 | /camera/depth/points 49 | camera_link 50 | 0.05 51 | 0 52 | 0 53 | 0 54 | 0 55 | 0 56 | 0 57 | 0 58 | 0 59 | 0 60 | 0 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/kinect_ros/thumbnails/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/smart_grasping_sandbox/models/kinect_ros/thumbnails/0.png -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/robot_pilar/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_pilar 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Software 10 | software@shadowrobot.com 11 | 12 | 13 | 14 | Robot Pilar. 15 | 16 | 17 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/models/robot_pilar/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 0.0 -1.0 0.5 0 -0 0 5 | 6 | 7 | 1 8 | 9 | 0.145833 10 | 0 11 | 0 12 | 0.145833 13 | 0 14 | 0.125 15 | 16 | 17 | 18 | 19 | 20 | 0.1 21 | 1 22 | 23 | 24 | 10 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 0.1 42 | 1 43 | 44 | 45 | 46 | 50 | 1 1 1 1 51 | 52 | 53 | 0 54 | 0 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/nodes/README.md: -------------------------------------------------------------------------------- 1 | This folder contains a few scripts I run on the sandbox to get data etc. Use at your own risk :) 2 | 3 | # Grasp Quality 4 | The grasp_quality.py runs a list of experiments and records data 5 | to gather a data set for a grasp quality metrics using machine learning. 6 | 7 | It grasps an object, shakes it and computes the distance between the object and 8 | the palm to have an objective grasp quality. 9 | 10 | To run it, you can use (first number is the distance you go down towards the ball, second number is the number of trials): 11 | 12 | ``` 13 | python grasp_quality.py -0.164 10 14 | ``` 15 | 16 | However to properly run it I really recommend creating a new Docker container with 17 | `grasp_quality.py` as its entrypoint. This way you can run it multiple times in 18 | parallel on a server for example. 19 | 20 | You can find an example entrypoint in `nodes/grasp_quality_entrypoint.sh`. 21 | 22 | The following command will run quite a few iterations with different approach distances from the ball: 23 | 24 | ``` 25 | for j in `seq 100`; do for i in -0.1645 -0.164 -0.1635 -0.163 -0.162 -0.161 -0.140; do docker run --rm -it -v /big/grasp_results_latest:/results --entrypoint /workspace/src/smart_grasping_sandbox/nodes/grasp_quality_entrypoint.sh shadowrobot/smart_grasping_sandbox $i 100 ; done; done 26 | ``` 27 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/nodes/grasp_quality.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2018 Shadow Robot Company Ltd - All Rights Reserved. 2 | # Proprietary and Confidential. Unauthorized copying of the content in this file, via any medium is strictly prohibited. 3 | 4 | from smart_grasping_sandbox.smart_grasper import SmartGrasper 5 | from tf.transformations import quaternion_from_euler 6 | from math import pi 7 | import time 8 | import rospy 9 | from math import sqrt, pow 10 | import random 11 | from sys import argv 12 | import uuid 13 | 14 | sgs = SmartGrasper() 15 | 16 | MIN_LIFT_STEPS = 5 17 | MAX_BALL_DISTANCE = 0.6 18 | 19 | CLOSED_HAND = {} 20 | 21 | CLOSED_HAND["H1_F1J1"] = 0.0 22 | CLOSED_HAND["H1_F1J2"] = 0.25 23 | CLOSED_HAND["H1_F1J3"] = 0.4 24 | CLOSED_HAND["H1_F2J1"] = 0.0 25 | CLOSED_HAND["H1_F2J2"] = 0.25 26 | CLOSED_HAND["H1_F2J3"] = 0.4 27 | CLOSED_HAND["H1_F3J1"] = 0.0 28 | CLOSED_HAND["H1_F3J2"] = 0.25 29 | CLOSED_HAND["H1_F3J3"] = 0.4 30 | 31 | JOINT_NAMES = CLOSED_HAND.keys() 32 | 33 | 34 | class GraspQuality(object): 35 | def __init__(self, sgs): 36 | self.sgs = sgs 37 | self.last_distance = None 38 | 39 | def check_stable(self, joint_names): 40 | current_min = 1000 41 | positions = [] 42 | velocities = [] 43 | efforts = [] 44 | for k in range(30): 45 | sgs.move_tip(y=0.02) 46 | ball_distance = self.__compute_euclidean_distance() 47 | if k > MIN_LIFT_STEPS and ball_distance < current_min: 48 | current_min = ball_distance 49 | if ball_distance > MAX_BALL_DISTANCE: 50 | break 51 | joints_positions, joints_velocity, joints_effort = self.sgs.get_current_joint_state() 52 | 53 | new_pos = [] 54 | new_vel = [] 55 | new_eff = [] 56 | for name in joint_names: 57 | new_pos.append(joints_positions[name]) 58 | new_vel.append(joints_velocity[name]) 59 | new_eff.append(joints_effort[name]) 60 | positions.append(new_pos) 61 | velocities.append(new_vel) 62 | efforts.append(new_eff) 63 | 64 | time.sleep(0.01) 65 | robustness = (1/(current_min - 0.18))**2 66 | return robustness, positions, velocities, efforts 67 | 68 | def __compute_euclidean_distance(self): 69 | ball_pose = self.sgs.get_object_pose() 70 | hand_pose = self.sgs.get_tip_pose() 71 | dist = sqrt((hand_pose.position.x - ball_pose.position.x)**2 + 72 | (hand_pose.position.y - ball_pose.position.y)**2 + 73 | (hand_pose.position.z - ball_pose.position.z)**2) 74 | return dist 75 | 76 | quality = GraspQuality(sgs) 77 | 78 | 79 | def experiment(grasp_distance=-0.163): 80 | sgs.reset_world() 81 | time.sleep(0.1) 82 | sgs.reset_world() 83 | time.sleep(0.1) 84 | 85 | sgs.open_hand() 86 | time.sleep(0.1) 87 | sgs.open_hand() 88 | time.sleep(0.1) 89 | 90 | ball_pose = sgs.get_object_pose() 91 | ball_pose.position.z += 0.5 92 | 93 | # setting an absolute orientation (from the top) 94 | quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) 95 | ball_pose.orientation.x = quaternion[0] 96 | ball_pose.orientation.y = quaternion[1] 97 | ball_pose.orientation.z = quaternion[2] 98 | ball_pose.orientation.w = quaternion[3] 99 | 100 | sgs.move_tip_absolute(ball_pose) 101 | 102 | sgs.move_tip(y=grasp_distance) 103 | 104 | # close the grasp 105 | sgs.check_fingers_collisions(False) 106 | 107 | random_closed_hand = {} 108 | for joint in CLOSED_HAND: 109 | random_closed_hand[joint] = CLOSED_HAND[joint] + random.gauss(0.0, 0.04) 110 | 111 | sgs.send_command(random_closed_hand, duration=1.0) 112 | 113 | # lift slowly and check the quality 114 | joint_names = random_closed_hand.keys() 115 | joint_targets = random_closed_hand.values() 116 | 117 | robustness, positions, velocities, efforts = quality.check_stable(joint_names) 118 | 119 | rospy.loginfo("Grasp quality = " + str(robustness)) 120 | 121 | sgs.check_fingers_collisions(True) 122 | return joint_names, joint_targets, robustness, positions, velocities, efforts 123 | 124 | 125 | with open("/results/headers.txt", "wb") as txt_file: 126 | headers = "experiment_number; robustness; " 127 | for name in JOINT_NAMES: 128 | headers += name+"_pos ; "+name+"_vel ; "+name+"_eff ; " 129 | headers += "measurement_number\n" 130 | 131 | txt_file.write(headers) 132 | 133 | grasp_distances = [float(i) for i in argv[1:-1]] 134 | number_of_tests_per_distance = int(argv[-1]) 135 | print "Running the grasp script with the distances: ", \ 136 | grasp_distances, " / number of tests: ", number_of_tests_per_distance 137 | 138 | for dist in grasp_distances: 139 | for _ in range(number_of_tests_per_distance): 140 | rospy.loginfo("---- grasping [" + str(uuid.uuid4().hex) + "/" + 141 | str(len(grasp_distances*number_of_tests_per_distance)) + 142 | "] - dist="+str(dist)) 143 | joint_names, joint_targets, robustness, positions, velocities, efforts = experiment(dist) 144 | 145 | with open("/results/"+str(uuid.uuid4())+".txt", "a") as txt_file: 146 | base_line = str(uuid.uuid4().hex)+" ; "+str(robustness) 147 | 148 | for measurement_number in range(len(positions)): 149 | pos = positions[measurement_number] 150 | vel = velocities[measurement_number] 151 | eff = efforts[measurement_number] 152 | 153 | measurement_line = base_line + " ; " 154 | for id_name, _ in enumerate(joint_names): 155 | measurement_line += str(pos[id_name]) + " ; " + str(vel[id_name]) + " ; " + str(eff[id_name]) + " ;" 156 | measurement_line += str(measurement_number) + "\n" 157 | 158 | txt_file.write(measurement_line) 159 | measurement_number += 1 160 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/nodes/grasp_quality_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | nohup Xvfb :1 -screen 0 1024x768x16 &> xvfb.log & 5 | DISPLAY=:1.0 6 | export DISPLAY 7 | 8 | source "/workspace/devel/setup.bash" 9 | source "/usr/share/gazebo/setup.sh" 10 | 11 | roslaunch smart_grasping_sandbox smart_grasping_sandbox.launch gui:=false gzweb:=true verbose:=true & 12 | 13 | sleep 20 14 | 15 | python /workspace/src/smart_grasping_sandbox/nodes/grasp_quality.py $1 $2 16 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/notebooks/Grasp Bayesian Optimization.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Grasp Bayesian Optimization\n", 8 | "\n", 9 | "The goal is to robustify a grasp that has been shown to the robot using bayesian optimization." 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": 1, 15 | "metadata": {}, 16 | "outputs": [ 17 | { 18 | "name": "stdout", 19 | "output_type": "stream", 20 | "text": [ 21 | "[INFO] [WallTime: 1504180219.210028] [12867.310000] STARTING CONTROLLERS\n" 22 | ] 23 | } 24 | ], 25 | "source": [ 26 | "from bayes_opt import BayesianOptimization\n", 27 | "from smart_grasping_sandbox.smart_grasper import SmartGrasper\n", 28 | "from tf.transformations import quaternion_from_euler\n", 29 | "from math import pi\n", 30 | "import time\n", 31 | "import rospy\n", 32 | "from math import sqrt, pow\n", 33 | "import random\n", 34 | "from sys import argv\n", 35 | "from numpy import var, mean\n", 36 | "\n", 37 | "\n", 38 | "sgs = SmartGrasper()\n", 39 | "\n", 40 | "MIN_LIFT_STEPS = 5\n", 41 | "MAX_BALL_DISTANCE = 0.6\n", 42 | "\n", 43 | "REPEAT_GRASP = 10" 44 | ] 45 | }, 46 | { 47 | "cell_type": "markdown", 48 | "metadata": {}, 49 | "source": [ 50 | "## Creating our function to optimize\n", 51 | "\n", 52 | "So we want to optimise the grasp stability function. This function will use the Smart Grasping Sandbox to grasp a ball, shake it and compute the stability of the grasp: the less the distance from the ball to the palm changes, the more stable the grasp." 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": 5, 58 | "metadata": { 59 | "collapsed": true 60 | }, 61 | "outputs": [], 62 | "source": [ 63 | "class GraspQuality(object):\n", 64 | " \n", 65 | " def __init__(self, sgs):\n", 66 | " self.sgs = sgs\n", 67 | " self.last_distance = None\n", 68 | " self.current_grasp = {}\n", 69 | "\n", 70 | " def check_stable(self, joint_names):\n", 71 | " current_min = 1000\n", 72 | " positions = []\n", 73 | " velocities = []\n", 74 | " efforts = []\n", 75 | " for k in range(30):\n", 76 | " sgs.move_tip(y=0.02)\n", 77 | " ball_distance = self.__compute_euclidean_distance()\n", 78 | " if k > MIN_LIFT_STEPS and ball_distance < current_min:\n", 79 | " current_min = ball_distance\n", 80 | " if ball_distance > MAX_BALL_DISTANCE:\n", 81 | " break\n", 82 | " \n", 83 | " time.sleep(0.01)\n", 84 | " robustness = (1/(current_min - 0.18))**2\n", 85 | " return robustness\n", 86 | "\n", 87 | " def __compute_euclidean_distance(self):\n", 88 | " ball_pose = self.sgs.get_object_pose()\n", 89 | " hand_pose = self.sgs.get_tip_pose()\n", 90 | " dist = sqrt((hand_pose.position.x - ball_pose.position.x)**2 + \\\n", 91 | " (hand_pose.position.y - ball_pose.position.y)**2 + \\\n", 92 | " (hand_pose.position.z - ball_pose.position.z)**2)\n", 93 | " return dist\n", 94 | " \n", 95 | " def run_experiments(self, grasp_distance,\n", 96 | " H1_F1J1, H1_F1J2, H1_F1J3,\n", 97 | " H1_F2J1, H1_F2J2, H1_F2J3,\n", 98 | " H1_F3J1, H1_F3J2, H1_F3J3):\n", 99 | " robustness = []\n", 100 | " for _ in range(REPEAT_GRASP):\n", 101 | " robustness.append(self.experiment(grasp_distance,\n", 102 | " H1_F1J1, H1_F1J2, H1_F1J3,\n", 103 | " H1_F2J1, H1_F2J2, H1_F2J3,\n", 104 | " H1_F3J1, H1_F3J2, H1_F3J3))\n", 105 | " \n", 106 | " # trying to maximize the robustness average - while minimizing its variance\n", 107 | " utility = mean(robustness) / max(0.001,sqrt(var(robustness))) # don't divide by 0\n", 108 | " \n", 109 | " return utility\n", 110 | " \n", 111 | " def experiment(self, grasp_distance,\n", 112 | " H1_F1J1, H1_F1J2, H1_F1J3,\n", 113 | " H1_F2J1, H1_F2J2, H1_F2J3,\n", 114 | " H1_F3J1, H1_F3J2, H1_F3J3):\n", 115 | " self.sgs.reset_world()\n", 116 | " time.sleep(0.1)\n", 117 | " self.sgs.reset_world()\n", 118 | " time.sleep(0.1)\n", 119 | "\n", 120 | " self.sgs.open_hand()\n", 121 | " time.sleep(0.1)\n", 122 | " self.sgs.open_hand()\n", 123 | " time.sleep(0.1)\n", 124 | "\n", 125 | " ball_pose = self.sgs.get_object_pose()\n", 126 | " ball_pose.position.z += 0.5\n", 127 | "\n", 128 | " #setting an absolute orientation (from the top)\n", 129 | " quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)\n", 130 | " ball_pose.orientation.x = quaternion[0]\n", 131 | " ball_pose.orientation.y = quaternion[1]\n", 132 | " ball_pose.orientation.z = quaternion[2]\n", 133 | " ball_pose.orientation.w = quaternion[3]\n", 134 | "\n", 135 | " self.sgs.move_tip_absolute(ball_pose)\n", 136 | "\n", 137 | " self.sgs.move_tip(y=grasp_distance)\n", 138 | "\n", 139 | " # close the grasp\n", 140 | " self.sgs.check_fingers_collisions(False)\n", 141 | " \n", 142 | " self.current_grasp[\"H1_F1J1\"] = H1_F1J1\n", 143 | " self.current_grasp[\"H1_F1J2\"] = H1_F1J2\n", 144 | " self.current_grasp[\"H1_F1J3\"] = H1_F1J3\n", 145 | " \n", 146 | " self.current_grasp[\"H1_F2J1\"] = H1_F2J1\n", 147 | " self.current_grasp[\"H1_F2J2\"] = H1_F2J2\n", 148 | " self.current_grasp[\"H1_F2J3\"] = H1_F2J3\n", 149 | " \n", 150 | " self.current_grasp[\"H1_F3J1\"] = H1_F3J1\n", 151 | " self.current_grasp[\"H1_F3J2\"] = H1_F3J2\n", 152 | " self.current_grasp[\"H1_F3J3\"] = H1_F3J3\n", 153 | " \n", 154 | " \n", 155 | " self.sgs.send_command(self.current_grasp, duration=1.0)\n", 156 | "\n", 157 | " # lift slowly and check the quality\n", 158 | " joint_names = self.current_grasp.keys()\n", 159 | "\n", 160 | " robustness = self.check_stable(joint_names)\n", 161 | "\n", 162 | " rospy.loginfo(\"Grasp quality = \" + str(robustness))\n", 163 | "\n", 164 | " sgs.check_fingers_collisions(True)\n", 165 | " \n", 166 | " return robustness" 167 | ] 168 | }, 169 | { 170 | "cell_type": "markdown", 171 | "metadata": {}, 172 | "source": [ 173 | "## Running the Bayesian Optimization\n", 174 | "\n", 175 | "And we can now optimize this black box function." 176 | ] 177 | }, 178 | { 179 | "cell_type": "code", 180 | "execution_count": 6, 181 | "metadata": {}, 182 | "outputs": [ 183 | { 184 | "name": "stdout", 185 | "output_type": "stream", 186 | "text": [ 187 | "[INFO] [WallTime: 1504180384.379762] [13031.070000] STARTING CONTROLLERS\n" 188 | ] 189 | } 190 | ], 191 | "source": [ 192 | "sgs = SmartGrasper()\n", 193 | "grasp_quality = GraspQuality(sgs)\n", 194 | "\n", 195 | "bo = BayesianOptimization(grasp_quality.run_experiments,\n", 196 | " {\"grasp_distance\": (-0.165, -0.16),\n", 197 | " \"H1_F1J1\": (-0.2, 0.2),\n", 198 | " \"H1_F1J2\": (0.0, 0.5),\n", 199 | " \"H1_F1J3\": (0.2, 0.6),\n", 200 | " \"H1_F2J1\": (-0.2, 0.2),\n", 201 | " \"H1_F2J2\": (0.0, 0.5),\n", 202 | " \"H1_F2J3\": (0.2, 0.6),\n", 203 | " \"H1_F3J1\": (-0.2, 0.2),\n", 204 | " \"H1_F3J2\": (0.0, 0.5),\n", 205 | " \"H1_F3J3\": (0.2, 0.6)\n", 206 | " })" 207 | ] 208 | }, 209 | { 210 | "cell_type": "code", 211 | "execution_count": 7, 212 | "metadata": {}, 213 | "outputs": [ 214 | { 215 | "name": "stdout", 216 | "output_type": "stream", 217 | "text": [ 218 | "\u001b[31mInitialization\u001b[0m\n", 219 | "\u001b[94m------------------------------------------------------------------------------------------------------------------------------------------------------------\u001b[0m\n", 220 | " Step | Time | Value | H1_F1J1 | H1_F1J2 | H1_F1J3 | H1_F2J1 | H1_F2J2 | H1_F2J3 | H1_F3J1 | H1_F3J2 | H1_F3J3 | grasp_distance | \n", 221 | "[INFO] [WallTime: 1504180386.738352] [13033.318000] STARTING CONTROLLERS\n", 222 | "[INFO] [WallTime: 1504180386.947866] [13033.422000] STARTING CONTROLLERS\n", 223 | "[INFO] [WallTime: 1504180399.795376] [13046.135000] Grasp quality = 11.360527161\n", 224 | "[INFO] [WallTime: 1504180400.803948] [13047.141000] STARTING CONTROLLERS\n", 225 | "[INFO] [WallTime: 1504180401.013203] [13047.247000] STARTING CONTROLLERS\n", 226 | "[INFO] [WallTime: 1504180413.834020] [13059.926000] Grasp quality = 11.3301113649\n", 227 | " 1 | 00m28s | \u001b[35m 746.01495\u001b[0m | \u001b[32m 0.0223\u001b[0m | \u001b[32m 0.0028\u001b[0m | \u001b[32m 0.3350\u001b[0m | \u001b[32m -0.0177\u001b[0m | \u001b[32m 0.1817\u001b[0m | \u001b[32m 0.5527\u001b[0m | \u001b[32m 0.1296\u001b[0m | \u001b[32m 0.3237\u001b[0m | \u001b[32m 0.3247\u001b[0m | \u001b[32m -0.1622\u001b[0m | \n", 228 | "\u001b[31mBayesian Optimization\u001b[0m\n", 229 | "\u001b[94m------------------------------------------------------------------------------------------------------------------------------------------------------------\u001b[0m\n", 230 | " Step | Time | Value | H1_F1J1 | H1_F1J2 | H1_F1J3 | H1_F2J1 | H1_F2J2 | H1_F2J3 | H1_F3J1 | H1_F3J2 | H1_F3J3 | grasp_distance | \n", 231 | "[INFO] [WallTime: 1504180421.060900] [13067.115000] STARTING CONTROLLERS\n", 232 | "[INFO] [WallTime: 1504180421.270110] [13067.221000] STARTING CONTROLLERS\n", 233 | "[INFO] [WallTime: 1504180434.103532] [13079.919000] Grasp quality = 11.6838141337\n", 234 | "[INFO] [WallTime: 1504180435.116533] [13080.926000] STARTING CONTROLLERS\n", 235 | "[INFO] [WallTime: 1504180435.325135] [13081.031000] STARTING CONTROLLERS\n", 236 | "[INFO] [WallTime: 1504180448.164399] [13093.742000] Grasp quality = 11.7927582576\n", 237 | " 2 | 00m35s | 215.49186 | 0.0235 | 0.0024 | 0.3358 | -0.0190 | 0.1804 | 0.5519 | 0.1299 | 0.3252 | 0.3236 | -0.1622 | \n", 238 | "[INFO] [WallTime: 1504180449.945942] [13095.505000] STARTING CONTROLLERS\n", 239 | "[INFO] [WallTime: 1504180450.154511] [13095.610000] STARTING CONTROLLERS\n", 240 | "[INFO] [WallTime: 1504180463.421428] [13108.747000] Grasp quality = 13.2227681217\n", 241 | "[INFO] [WallTime: 1504180464.436342] [13109.755000] STARTING CONTROLLERS\n", 242 | "[INFO] [WallTime: 1504180464.646297] [13109.861000] STARTING CONTROLLERS\n", 243 | "[INFO] [WallTime: 1504180478.012128] [13123.098000] Grasp quality = 14.6425203273\n", 244 | " 3 | 00m29s | 19.62687 | -0.0463 | 0.2396 | 0.3119 | 0.0880 | 0.4517 | 0.5973 | -0.1170 | 0.0421 | 0.4696 | -0.1619 | \n" 245 | ] 246 | } 247 | ], 248 | "source": [ 249 | "bo.maximize(init_points=15, n_iter=10, kappa=4)\n", 250 | "\n" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": 8, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "best_grasp = bo.res['max']['max_params']" 260 | ] 261 | }, 262 | { 263 | "cell_type": "code", 264 | "execution_count": 9, 265 | "metadata": {}, 266 | "outputs": [ 267 | { 268 | "name": "stdout", 269 | "output_type": "stream", 270 | "text": [ 271 | "[INFO] [WallTime: 1504180544.912030] [13189.813000] STARTING CONTROLLERS\n", 272 | "[INFO] [WallTime: 1504180545.120841] [13189.919000] STARTING CONTROLLERS\n", 273 | "[INFO] [WallTime: 1504180558.061862] [13202.735000] Grasp quality = 11.7715838606\n" 274 | ] 275 | }, 276 | { 277 | "data": { 278 | "text/plain": [ 279 | "11.771583860585748" 280 | ] 281 | }, 282 | "execution_count": 9, 283 | "metadata": {}, 284 | "output_type": "execute_result" 285 | } 286 | ], 287 | "source": [ 288 | "grasp_quality.experiment(best_grasp[\"grasp_distance\"],\n", 289 | " best_grasp[\"H1_F1J1\"], best_grasp[\"H1_F1J2\"], best_grasp[\"H1_F1J3\"],\n", 290 | " best_grasp[\"H1_F2J1\"], best_grasp[\"H1_F2J2\"], best_grasp[\"H1_F2J3\"],\n", 291 | " best_grasp[\"H1_F3J1\"], best_grasp[\"H1_F3J2\"], best_grasp[\"H1_F3J3\"],\n", 292 | " )" 293 | ] 294 | } 295 | ], 296 | "metadata": { 297 | "kernelspec": { 298 | "display_name": "Python 2", 299 | "language": "python", 300 | "name": "python2" 301 | }, 302 | "language_info": { 303 | "codemirror_mode": { 304 | "name": "ipython", 305 | "version": 2 306 | }, 307 | "file_extension": ".py", 308 | "mimetype": "text/x-python", 309 | "name": "python", 310 | "nbconvert_exporter": "python", 311 | "pygments_lexer": "ipython2", 312 | "version": "2.7.6" 313 | }, 314 | "toc": { 315 | "nav_menu": {}, 316 | "number_sections": true, 317 | "sideBar": true, 318 | "skip_h1_title": false, 319 | "toc_cell": false, 320 | "toc_position": {}, 321 | "toc_section_display": "block", 322 | "toc_window_display": false 323 | } 324 | }, 325 | "nbformat": 4, 326 | "nbformat_minor": 2 327 | } 328 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/notebooks/Smart Grasping Sandbox.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": { 6 | "collapsed": true 7 | }, 8 | "source": [ 9 | "# Loading the library\n", 10 | "\n", 11 | "The Smart Grasping Sandbox comes with a helper library that makes it easy to interact with the environment." 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": 1, 17 | "metadata": {}, 18 | "outputs": [ 19 | { 20 | "name": "stdout", 21 | "output_type": "stream", 22 | "text": [ 23 | "[INFO] [WallTime: 1503476962.996059] [195.668000] STARTING CONTROLLERS\n" 24 | ] 25 | } 26 | ], 27 | "source": [ 28 | "from smart_grasping_sandbox.smart_grasper import SmartGrasper\n", 29 | "from tf.transformations import quaternion_from_euler\n", 30 | "from math import pi\n", 31 | "import time\n", 32 | "\n", 33 | "sgs = SmartGrasper()" 34 | ] 35 | }, 36 | { 37 | "cell_type": "markdown", 38 | "metadata": {}, 39 | "source": [ 40 | "# Pick the ball\n", 41 | "\n", 42 | "Try to pick the ball using the `pick` function." 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": 9, 48 | "metadata": {}, 49 | "outputs": [ 50 | { 51 | "name": "stdout", 52 | "output_type": "stream", 53 | "text": [ 54 | "[INFO] [WallTime: 1489489274.481599] [568.390000] Moving to Pregrasp\n", 55 | "[INFO] [WallTime: 1489489277.343799] [570.319000] Grasping\n", 56 | "[INFO] [WallTime: 1489489281.400402] [573.092000] Lifting\n" 57 | ] 58 | } 59 | ], 60 | "source": [ 61 | "sgs.pick()" 62 | ] 63 | }, 64 | { 65 | "cell_type": "code", 66 | "execution_count": 53, 67 | "metadata": {}, 68 | "outputs": [ 69 | { 70 | "name": "stdout", 71 | "output_type": "stream", 72 | "text": [ 73 | "[INFO] [WallTime: 1503477694.247437] [924.720000] STARTING CONTROLLERS\n" 74 | ] 75 | } 76 | ], 77 | "source": [ 78 | "sgs.reset_world()" 79 | ] 80 | }, 81 | { 82 | "cell_type": "markdown", 83 | "metadata": {}, 84 | "source": [ 85 | "## A bit more detail\n", 86 | "This is what happens in the `pick` function. \n", 87 | "\n", 88 | "First we move over the ball" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": 55, 94 | "metadata": {}, 95 | "outputs": [ 96 | { 97 | "data": { 98 | "text/plain": [ 99 | "True" 100 | ] 101 | }, 102 | "execution_count": 55, 103 | "metadata": {}, 104 | "output_type": "execute_result" 105 | } 106 | ], 107 | "source": [ 108 | "sgs.open_hand()\n", 109 | "time.sleep(0.1)\n", 110 | "\n", 111 | "ball_pose = sgs.get_object_pose()\n", 112 | "ball_pose.position.z += 0.5\n", 113 | "\n", 114 | "#setting an absolute orientation (from the top)\n", 115 | "quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)\n", 116 | "ball_pose.orientation.x = quaternion[0]\n", 117 | "ball_pose.orientation.y = quaternion[1]\n", 118 | "ball_pose.orientation.z = quaternion[2]\n", 119 | "ball_pose.orientation.w = quaternion[3]\n", 120 | "\n", 121 | "sgs.move_tip_absolute(ball_pose)\n" 122 | ] 123 | }, 124 | { 125 | "cell_type": "markdown", 126 | "metadata": {}, 127 | "source": [ 128 | "Now finalise the approach and close the hand" 129 | ] 130 | }, 131 | { 132 | "cell_type": "code", 133 | "execution_count": 56, 134 | "metadata": {}, 135 | "outputs": [ 136 | { 137 | "data": { 138 | "text/plain": [ 139 | "True" 140 | ] 141 | }, 142 | "execution_count": 56, 143 | "metadata": {}, 144 | "output_type": "execute_result" 145 | } 146 | ], 147 | "source": [ 148 | "\n", 149 | "sgs.check_fingers_collisions(True)\n", 150 | "sgs.move_tip(y=-0.164)" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": 57, 156 | "metadata": {}, 157 | "outputs": [ 158 | { 159 | "data": { 160 | "text/plain": [ 161 | "False" 162 | ] 163 | }, 164 | "execution_count": 57, 165 | "metadata": {}, 166 | "output_type": "execute_result" 167 | } 168 | ], 169 | "source": [ 170 | "sgs.check_fingers_collisions(False)\n", 171 | "sgs.close_hand()" 172 | ] 173 | }, 174 | { 175 | "cell_type": "markdown", 176 | "metadata": {}, 177 | "source": [ 178 | "And finally lift the ball" 179 | ] 180 | }, 181 | { 182 | "cell_type": "code", 183 | "execution_count": 58, 184 | "metadata": {}, 185 | "outputs": [ 186 | { 187 | "data": { 188 | "text/plain": [ 189 | "True" 190 | ] 191 | }, 192 | "execution_count": 58, 193 | "metadata": {}, 194 | "output_type": "execute_result" 195 | } 196 | ], 197 | "source": [ 198 | "for _ in range(5):\n", 199 | " sgs.move_tip(y=0.01)\n", 200 | " time.sleep(0.1)\n", 201 | " \n", 202 | "sgs.check_fingers_collisions(True)" 203 | ] 204 | }, 205 | { 206 | "cell_type": "markdown", 207 | "metadata": {}, 208 | "source": [ 209 | "## Other tricks\n", 210 | "\n", 211 | "We can also send direct commands to the different joints." 212 | ] 213 | }, 214 | { 215 | "cell_type": "code", 216 | "execution_count": 5, 217 | "metadata": {}, 218 | "outputs": [ 219 | { 220 | "name": "stdout", 221 | "output_type": "stream", 222 | "text": [ 223 | "[INFO] [WallTime: 1488970296.130442] [229.709000] STARTING CONTROLLERS\n" 224 | ] 225 | } 226 | ], 227 | "source": [ 228 | "sgs.reset_world()\n", 229 | "\n", 230 | "sgs.send_command({\"H1_F1J1\": -1.0, \"shoulder_pan_joint\": 2.0})\n" 231 | ] 232 | }, 233 | { 234 | "cell_type": "code", 235 | "execution_count": 3, 236 | "metadata": {}, 237 | "outputs": [ 238 | { 239 | "name": "stdout", 240 | "output_type": "stream", 241 | "text": [ 242 | "Latest joints position: {'H1_F1J2': 0.0031896331811251954, 'H1_F1J1': -0.992757365708627, 'elbow_joint': -1.4999998102410448, 'shoulder_pan_joint': 1.2000856167965388, 'H1_F3J1': -0.012612651032773314, 'H1_F3J2': -0.005294463440041142, 'wrist_3_joint': -0.30000041150587275, 'wrist_1_joint': -0.5000002077251793, 'shoulder_lift_joint': 0.5000077397819904, 'H1_WRJ1': -0.008405688120210364, 'H1_F2J1': -0.017007151082448324, 'wrist_2_joint': -1.6000023499398974, 'H1_F2J2': -1.2585362279438073e-05}\n" 243 | ] 244 | } 245 | ], 246 | "source": [ 247 | "joints_position, joints_velocity, joints_effort = sgs.get_current_joint_state()\n", 248 | "print \"Latest joints position: \", joints_position" 249 | ] 250 | }, 251 | { 252 | "cell_type": "code", 253 | "execution_count": null, 254 | "metadata": { 255 | "collapsed": true 256 | }, 257 | "outputs": [], 258 | "source": [ 259 | "sgs.swap_object(\"beer\")" 260 | ] 261 | } 262 | ], 263 | "metadata": { 264 | "kernelspec": { 265 | "display_name": "Python 2", 266 | "language": "python", 267 | "name": "python2" 268 | }, 269 | "language_info": { 270 | "codemirror_mode": { 271 | "name": "ipython", 272 | "version": 2 273 | }, 274 | "file_extension": ".py", 275 | "mimetype": "text/x-python", 276 | "name": "python", 277 | "nbconvert_exporter": "python", 278 | "pygments_lexer": "ipython2", 279 | "version": "2.7.6" 280 | }, 281 | "toc": { 282 | "colors": { 283 | "hover_highlight": "#DAA520", 284 | "navigate_num": "#000000", 285 | "navigate_text": "#333333", 286 | "running_highlight": "#FF0000", 287 | "selected_highlight": "#FFD700", 288 | "sidebar_border": "#EEEEEE", 289 | "wrapper_background": "#FFFFFF" 290 | }, 291 | "moveMenuLeft": true, 292 | "nav_menu": { 293 | "height": "30px", 294 | "width": "252px" 295 | }, 296 | "navigate_menu": true, 297 | "number_sections": true, 298 | "sideBar": true, 299 | "threshold": 4, 300 | "toc_cell": false, 301 | "toc_section_display": "block", 302 | "toc_window_display": false, 303 | "widenNotebook": false 304 | } 305 | }, 306 | "nbformat": 4, 307 | "nbformat_minor": 1 308 | } 309 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/notebooks/model-Q.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/smart_grasping_sandbox/notebooks/model-Q.h5 -------------------------------------------------------------------------------- /smart_grasping_sandbox/notebooks/model.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/smart_grasping_sandbox/notebooks/model.h5 -------------------------------------------------------------------------------- /smart_grasping_sandbox/notebooks/state.pckl: -------------------------------------------------------------------------------- 1 | (lp0 2 | F0.8035899673005142 3 | aF0.4266817311577247 4 | aF0.5538231671975873 5 | aF0.02270309361153469 6 | aF0.2250564469831754 7 | aF0.015644407869805782 8 | aF0.10958480156794784 9 | aF0.25457224396700096 10 | aF0.6372528331318417 11 | aI0 12 | a. -------------------------------------------------------------------------------- /smart_grasping_sandbox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | smart_grasping_sandbox 4 | 0.0.0 5 | A package that contains the main launch file for running a sandbox environment for Shadow's Smart Grasping System. 6 | 7 | Shadow Software team 8 | 9 | GPLv3 10 | 11 | Ugo Cupcic 12 | 13 | catkin 14 | rospy 15 | fh_desc 16 | smart_grasp_moveit_config 17 | rospy 18 | fh_desc 19 | smart_grasp_moveit_config 20 | 21 | 22 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=['smart_grasping_sandbox'], 6 | scripts=[], 7 | package_dir={'': 'src'} 8 | ) 9 | 10 | setup(**d) 11 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/src/smart_grasping_sandbox/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/smart_grasping_sandbox/6f8de0600126bcbd38c38c1fcc8b97c6a868ad8d/smart_grasping_sandbox/src/smart_grasping_sandbox/__init__.py -------------------------------------------------------------------------------- /smart_grasping_sandbox/src/smart_grasping_sandbox/smart_grasper.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2018 Shadow Robot Company Ltd - All Rights Reserved. 2 | # Proprietary and Confidential. Unauthorized copying of the content in this file, via any medium is strictly prohibited. 3 | 4 | import rospy 5 | from std_srvs.srv import Empty 6 | from gazebo_msgs.srv import GetModelState, SetModelConfiguration, DeleteModel, \ 7 | SpawnModel 8 | from geometry_msgs.msg import Pose 9 | from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest 10 | from moveit_msgs.msg import PlanningScene, PlanningSceneComponents 11 | from moveit_msgs.srv import GetPlanningScene 12 | from moveit_commander import MoveGroupCommander 13 | from actionlib import SimpleActionClient 14 | from control_msgs.msg import FollowJointTrajectoryAction, \ 15 | FollowJointTrajectoryGoal 16 | from sensor_msgs.msg import JointState 17 | from trajectory_msgs.msg import JointTrajectoryPoint 18 | from tf.transformations import quaternion_from_euler 19 | from math import pi 20 | from copy import deepcopy 21 | import time 22 | from tf_conversions import posemath, toMsg 23 | import PyKDL 24 | from threading import Timer 25 | 26 | 27 | class SmartGrasper(object): 28 | """ 29 | This is the helper library to easily access the different functionalities of the simulated robot 30 | from python. 31 | """ 32 | 33 | __last_joint_state = None 34 | __current_model_name = "cricket_ball" 35 | __path_to_models = "/root/.gazebo/models/" 36 | 37 | def __init__(self): 38 | """ 39 | This constructor initialises the different necessary connections to the topics and services 40 | and resets the world to start in a good position. 41 | """ 42 | rospy.init_node("smart_grasper") 43 | 44 | self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, 45 | self.__joint_state_cb, queue_size=1) 46 | 47 | rospy.wait_for_service("/gazebo/get_model_state", 10.0) 48 | rospy.wait_for_service("/gazebo/reset_world", 10.0) 49 | self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty) 50 | self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) 51 | 52 | rospy.wait_for_service("/gazebo/pause_physics") 53 | self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty) 54 | rospy.wait_for_service("/gazebo/unpause_physics") 55 | self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) 56 | rospy.wait_for_service("/controller_manager/switch_controller") 57 | self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController) 58 | rospy.wait_for_service("/gazebo/set_model_configuration") 59 | self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration) 60 | 61 | rospy.wait_for_service("/gazebo/delete_model") 62 | self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel) 63 | rospy.wait_for_service("/gazebo/spawn_sdf_model") 64 | self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel) 65 | 66 | rospy.wait_for_service('/get_planning_scene', 10.0) 67 | self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) 68 | self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True) 69 | 70 | self.arm_commander = MoveGroupCommander("arm") 71 | self.hand_commander = MoveGroupCommander("hand") 72 | 73 | self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", 74 | FollowJointTrajectoryAction) 75 | self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", 76 | FollowJointTrajectoryAction) 77 | 78 | if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: 79 | rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") 80 | raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") 81 | 82 | if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: 83 | rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") 84 | raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") 85 | 86 | self.reset_world() 87 | 88 | def reset_world(self): 89 | """ 90 | Resets the object poses in the world and the robot joint angles. 91 | """ 92 | self.__switch_ctrl.call(start_controllers=[], 93 | stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"], 94 | strictness=SwitchControllerRequest.BEST_EFFORT) 95 | self.__pause_physics.call() 96 | 97 | joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 98 | 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 99 | 'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3'] 100 | joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0] 101 | 102 | self.__set_model.call(model_name="smart_grasping_sandbox", 103 | urdf_param_name="robot_description", 104 | joint_names=joint_names, 105 | joint_positions=joint_positions) 106 | 107 | timer = Timer(0.0, self.__start_ctrl) 108 | timer.start() 109 | 110 | time.sleep(0.1) 111 | self.__unpause_physics.call() 112 | 113 | self.__reset_world.call() 114 | 115 | def get_object_pose(self): 116 | """ 117 | Gets the pose of the ball in the world frame. 118 | @return The pose of the ball. 119 | """ 120 | return self.__get_pose_srv.call(self.__current_model_name, "world").pose 121 | 122 | def get_tip_pose(self): 123 | """ 124 | Gets the current pose of the robot's tooltip in the world frame. 125 | @return the tip pose 126 | """ 127 | return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose 128 | 129 | def move_tip_absolute(self, target): 130 | """ 131 | Moves the tooltip to the absolute target in the world frame 132 | @param target is a geometry_msgs.msg.Pose 133 | @return True on success 134 | """ 135 | self.arm_commander.set_start_state_to_current_state() 136 | self.arm_commander.set_pose_targets([target]) 137 | plan = self.arm_commander.plan() 138 | if not self.arm_commander.execute(plan): 139 | return False 140 | return True 141 | 142 | def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): 143 | """ 144 | Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. 145 | @return True on success 146 | """ 147 | transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw), 148 | PyKDL.Vector(-x, -y, -z)) 149 | 150 | tip_pose = self.get_tip_pose() 151 | tip_pose_kdl = posemath.fromMsg(tip_pose) 152 | final_pose = toMsg(tip_pose_kdl * transform) 153 | 154 | self.arm_commander.set_start_state_to_current_state() 155 | self.arm_commander.set_pose_targets([final_pose]) 156 | plan = self.arm_commander.plan() 157 | if not self.arm_commander.execute(plan): 158 | return False 159 | return True 160 | 161 | def send_command(self, command, duration=0.2): 162 | """ 163 | Send a dictionnary of joint targets to the arm and hand directly. 164 | @param command: a dictionnary of joint names associated with a target: 165 | {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0} 166 | @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0 167 | """ 168 | hand_goal = None 169 | arm_goal = None 170 | 171 | for joint, target in command.items(): 172 | if "H1" in joint: 173 | if not hand_goal: 174 | hand_goal = FollowJointTrajectoryGoal() 175 | point = JointTrajectoryPoint() 176 | point.time_from_start = rospy.Duration.from_sec(duration) 177 | hand_goal.trajectory.points.append(point) 178 | hand_goal.trajectory.joint_names.append(joint) 179 | hand_goal.trajectory.points[0].positions.append(target) 180 | else: 181 | if not arm_goal: 182 | arm_goal = FollowJointTrajectoryGoal() 183 | point = JointTrajectoryPoint() 184 | point.time_from_start = rospy.Duration.from_sec(duration) 185 | arm_goal.trajectory.points.append(point) 186 | arm_goal.trajectory.joint_names.append(joint) 187 | arm_goal.trajectory.points[0].positions.append(target) 188 | if arm_goal: 189 | self.__arm_traj_client.send_goal_and_wait(arm_goal) 190 | if hand_goal: 191 | self.__hand_traj_client.send_goal_and_wait(hand_goal) 192 | 193 | def get_current_joint_state(self): 194 | """ 195 | Gets the current state of the robot. 196 | @return joint positions, velocity and efforts as three dictionnaries 197 | """ 198 | joints_position = {n: p for n, p in 199 | zip(self.__last_joint_state.name, 200 | self.__last_joint_state.position)} 201 | joints_velocity = {n: v for n, v in 202 | zip(self.__last_joint_state.name, 203 | self.__last_joint_state.velocity)} 204 | joints_effort = {n: v for n, v in 205 | zip(self.__last_joint_state.name, 206 | self.__last_joint_state.effort)} 207 | return joints_position, joints_velocity, joints_effort 208 | 209 | def open_hand(self): 210 | """ 211 | Opens the hand. 212 | @return True on success 213 | """ 214 | self.hand_commander.set_named_target("open") 215 | plan = self.hand_commander.plan() 216 | if not self.hand_commander.execute(plan, wait=True): 217 | return False 218 | return True 219 | 220 | def close_hand(self): 221 | """ 222 | Closes the hand. 223 | @return True on success 224 | """ 225 | self.hand_commander.set_named_target("close") 226 | plan = self.hand_commander.plan() 227 | if not self.hand_commander.execute(plan, wait=True): 228 | return False 229 | return True 230 | 231 | def check_fingers_collisions(self, enable=True): 232 | """ 233 | Disables or enables the collisions check between the fingers and the objects / table 234 | @param enable: set to True to enable / False to disable 235 | @return True on success 236 | """ 237 | objects = ["cricket_ball__link", "drill__link", "cafe_table__link"] 238 | 239 | while self.__pub_planning_scene.get_num_connections() < 1: 240 | rospy.loginfo("waiting for someone to subscribe to the /planning_scene") 241 | rospy.sleep(0.1) 242 | 243 | request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) 244 | response = self.__get_planning_scene(request) 245 | acm = response.scene.allowed_collision_matrix 246 | 247 | for object_name in objects: 248 | if object_name not in acm.entry_names: 249 | # add object to allowed collision matrix 250 | acm.entry_names += [object_name] 251 | for row in range(len(acm.entry_values)): 252 | acm.entry_values[row].enabled += [False] 253 | new_row = deepcopy(acm.entry_values[0]) 254 | acm.entry_values += {new_row} 255 | 256 | for index_entry_values, entry_values in enumerate(acm.entry_values): 257 | if "H1_F" in acm.entry_names[index_entry_values]: 258 | for index_value, _ in enumerate(entry_values.enabled): 259 | if acm.entry_names[index_value] in objects: 260 | if enable: 261 | acm.entry_values[index_entry_values].enabled[index_value] = False 262 | else: 263 | acm.entry_values[index_entry_values].enabled[index_value] = True 264 | elif acm.entry_names[index_entry_values] in objects: 265 | for index_value, _ in enumerate(entry_values.enabled): 266 | if "H1_F" in acm.entry_names[index_value]: 267 | if enable: 268 | acm.entry_values[index_entry_values].enabled[index_value] = False 269 | else: 270 | acm.entry_values[index_entry_values].enabled[index_value] = True 271 | planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) 272 | self.__pub_planning_scene.publish(planning_scene_diff) 273 | rospy.sleep(1.0) 274 | return True 275 | 276 | def pick(self): 277 | """ 278 | Does its best to pick the ball. 279 | """ 280 | rospy.loginfo("Moving to Pregrasp") 281 | self.open_hand() 282 | time.sleep(0.1) 283 | 284 | ball_pose = self.get_object_pose() 285 | ball_pose.position.z += 0.5 286 | 287 | # setting an absolute orientation (from the top) 288 | quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) 289 | ball_pose.orientation.x = quaternion[0] 290 | ball_pose.orientation.y = quaternion[1] 291 | ball_pose.orientation.z = quaternion[2] 292 | ball_pose.orientation.w = quaternion[3] 293 | 294 | self.move_tip_absolute(ball_pose) 295 | time.sleep(0.1) 296 | 297 | rospy.loginfo("Grasping") 298 | self.move_tip(y=-0.164) 299 | time.sleep(0.1) 300 | self.check_fingers_collisions(False) 301 | time.sleep(0.1) 302 | self.close_hand() 303 | time.sleep(0.1) 304 | 305 | rospy.loginfo("Lifting") 306 | for _ in range(5): 307 | self.move_tip(y=0.01) 308 | time.sleep(0.1) 309 | 310 | self.check_fingers_collisions(True) 311 | 312 | def swap_object(self, new_model_name): 313 | """ 314 | Replaces the current object with a new one.Replaces 315 | @new_model_name the name of the folder in which the object is (e.g. beer) 316 | """ 317 | try: 318 | self.__delete_model(self.__current_model_name) 319 | except: 320 | rospy.logwarn("Failed to delete: " + self.__current_model_name) 321 | try: 322 | sdf = None 323 | initial_pose = Pose() 324 | initial_pose.position.x = 0.15 325 | initial_pose.position.z = 0.82 326 | with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model: 327 | sdf = model.read() 328 | res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world") 329 | rospy.logerr("RES: " + str(res)) 330 | self.__current_model_name = new_model_name 331 | except: 332 | rospy.logwarn("Failed to delete: " + self.__current_model_name) 333 | 334 | def __compute_arm_target_for_ball(self): 335 | ball_pose = self.get_object_pose() 336 | 337 | # come at it from the top 338 | arm_target = ball_pose 339 | arm_target.position.z += 0.5 340 | 341 | quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) 342 | arm_target.orientation.x = quaternion[0] 343 | arm_target.orientation.y = quaternion[1] 344 | arm_target.orientation.z = quaternion[2] 345 | arm_target.orientation.w = quaternion[3] 346 | 347 | return arm_target 348 | 349 | def __pre_grasp(self, arm_target): 350 | self.hand_commander.set_named_target("open") 351 | plan = self.hand_commander.plan() 352 | self.hand_commander.execute(plan, wait=True) 353 | 354 | for _ in range(10): 355 | self.arm_commander.set_start_state_to_current_state() 356 | self.arm_commander.set_pose_targets([arm_target]) 357 | plan = self.arm_commander.plan() 358 | if self.arm_commander.execute(plan): 359 | return True 360 | 361 | def __grasp(self, arm_target): 362 | waypoints = [] 363 | waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) 364 | arm_above_ball = deepcopy(arm_target) 365 | arm_above_ball.position.z -= 0.12 366 | waypoints.append(arm_above_ball) 367 | 368 | self.arm_commander.set_start_state_to_current_state() 369 | (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) 370 | print fraction 371 | if not self.arm_commander.execute(plan): 372 | return False 373 | 374 | self.hand_commander.set_named_target("close") 375 | plan = self.hand_commander.plan() 376 | if not self.hand_commander.execute(plan, wait=True): 377 | return False 378 | 379 | self.hand_commander.attach_object("cricket_ball__link") 380 | 381 | def __lift(self, arm_target): 382 | waypoints = [] 383 | waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) 384 | arm_above_ball = deepcopy(arm_target) 385 | arm_above_ball.position.z += 0.1 386 | waypoints.append(arm_above_ball) 387 | 388 | self.arm_commander.set_start_state_to_current_state() 389 | (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) 390 | print fraction 391 | if not self.arm_commander.execute(plan): 392 | return False 393 | 394 | def __start_ctrl(self): 395 | rospy.loginfo("STARTING CONTROLLERS") 396 | self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], 397 | stop_controllers=[], strictness=1) 398 | 399 | def __joint_state_cb(self, msg): 400 | self.__last_joint_state = msg 401 | -------------------------------------------------------------------------------- /smart_grasping_sandbox/world/smart_grasping.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 0 0 -9.8 4 | 6e-06 2.3e-05 -4.2e-05 5 | 6 | 7 | 0.000000 0.000000 -9.810000 8 | 9 | 12 | 5 13 | 14 | 15 | 16 | quick 17 | 50 18 | 0 19 | 1.300000 20 | 21 | 22 | 0.000000 23 | 0.200000 24 | 100.000000 25 | 0.001000 26 | 27 | 28 | 29 | 30 | 0.4 0.4 0.4 1 31 | 0.7 0.7 1 1 32 | 1 33 | 34 | 35 | EARTH_WGS84 36 | 0 37 | 0 38 | 0 39 | 0 40 | 41 | 42 | 1 43 | 44 | 45 | 0 0 0.755 0 -0 0 46 | 47 | 48 | 0.913 0.913 0.04 49 | 50 | 51 | 10 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 0 0 0.37 0 -0 0 67 | 68 | 69 | 0.042 0.042 0.74 70 | 71 | 72 | 10 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 0 0 0.02 0 -0 0 88 | 89 | 90 | 0.56 0.56 0.04 91 | 92 | 93 | 10 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | model://cafe_table/meshes/cafe_table.dae 111 | 1 1 1 112 | 113 | 114 | 115 | 0 116 | 0 117 | 118 | -0.168004 0.136981 0 0 -0 0 119 | 120 | 121 | 122 | 123 | -0.00637 -0.008 0.13254 0 -0 0 124 | 125 | 0.0133113 126 | -0.00030365 127 | -0.00034148 128 | 0.0115766 129 | 0.00088073 130 | 0.00378028 131 | 132 | 1.50252 133 | 134 | 135 | 0 0 -0.09 0 -0 0 136 | 137 | 138 | model://cordless_drill/meshes/cordless_drill.stl 139 | 1 1 1 140 | 141 | 142 | 143 | 144 | 145 | 0.1 146 | 0.001 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 10 158 | 159 | 160 | 0 0 -0.09 0 -0 0 161 | 162 | 163 | model://cordless_drill/meshes/cordless_drill.dae 164 | 1 1 1 165 | 166 | 167 | 168 | 0 169 | 0 170 | 171 | -0.472191 0.158603 0.772337 0.000673 -0.000771 1e-06 172 | 173 | 174 | 175 | 0 0 0.0375 0 -0 0 176 | 177 | 0.1467 178 | 179 | 8.25187e-05 180 | 0 181 | 0 182 | 8.25187e-05 183 | 0 184 | 8.25187e-05 185 | 186 | 187 | 188 | 189 | 190 | 0.0375 191 | 192 | 193 | 194 | 195 | 0.347 196 | 8.8e+09 197 | 198 | 100000 199 | 100 200 | 100 201 | 0.001 202 | 203 | 204 | 205 | 206 | 1 207 | 0 208 | 0.01 209 | 210 | 211 | 212 | 213 | 214 | 215 | 10 216 | 217 | 218 | 219 | 220 | 0.0375 221 | 222 | 223 | 224 | 228 | 229 | 230 | 231 | 0 232 | 0.005 233 | 234 | 0 235 | 0 236 | 237 | 0.15003 -5.5e-05 0.82 0.003644 -0.00082 0.000309 238 | 239 | 240 | -0.614545 -0.753038 2.0714 0.392405 -0 0 241 | 0.7 0.5 0.2 1 242 | 0.1 0.1 0.1 1 243 | 0 0 -1 244 | 245 | 20 246 | 0.5 247 | 0.01 248 | 0.001 249 | 250 | 0 251 | 252 | 0.6 253 | 1 254 | 1 255 | 256 | 257 | 258 | 0 0 259 | 0 0 260 | 1480586122 155091523 261 | 0 262 | 263 | -0.168004 0.136981 0 0 -0 0 264 | 1 1 1 265 | 266 | -0.168004 0.136981 0 0 -0 0 267 | 0 0 0 0 -0 0 268 | 0 0 0 0 -0 0 269 | 0 0 0 0 -0 0 270 | 271 | 272 | 273 | 0.15003 -5.5e-05 0.82 0.003644 -0.00082 0.000309 274 | 1 1 1 275 | 276 | 0.15 -0.000192 0.835975 0.003644 -0.00082 0.000309 277 | 0 0 0 0 -0 0 278 | 0.00314 0.179937 -0.256644 2.55004 -1.5124 2.12055 279 | 0.000461 0.026397 -0.03765 0 -0 0 280 | 281 | 282 | 283 | -0.472191 0.158603 0.772337 0.000673 -0.000771 1e-06 284 | 1 1 1 285 | 286 | -0.472191 0.158603 0.772337 0.000673 -0.000771 1e-06 287 | 0 0 0 0 -0 0 288 | -0.265657 0.080101 -0.000779 2.52813 -1.10431 3.13382 289 | -0.399155 0.120353 -0.001171 0 -0 0 290 | 291 | 292 | 293 | 0 0 3 0 -0 0 294 | 295 | 296 | -0.614545 -0.753038 2.0714 0.392405 -0 0 297 | 298 | 299 | 300 | 301 | 3.12753 -1.41682 1.86723 0 0.279643 2.50817 302 | orbit 303 | perspective 304 | 305 | 306 | 307 | 0 0 3 0 -0 0 308 | 0.9 0.85 0.8 1 309 | 0.1 0.1 0.1 1 310 | 0.1 0.1 -0.9 311 | 312 | 20 313 | 0.5 314 | 0.01 315 | 0.001 316 | 317 | 1 318 | 319 | 320 | model://smart_grasping_sandbox/models/building 321 | true 322 | 0.25 0.18 0 0 -0 0 323 | 324 | 325 | model://smart_grasping_sandbox/models/floor 326 | true 327 | 0.0 0.0 -0.025 0 0 0 328 | 329 | 330 | model://smart_grasping_sandbox/models/kinect_pilar 331 | true 332 | 1.39 0.242 0.5 0 -0 0 333 | 334 | 335 | model://smart_grasping_sandbox/models/robot_pilar 336 | true 337 | 0.0 -1.0 0.5 0 -0 0 338 | 339 | 340 | 341 | 342 | --------------------------------------------------------------------------------