├── .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 | [](https://hub.docker.com/r/shadowrobot/smart_grasping_sandbox) |
4 | | Landscape | [](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 | 
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 |
--------------------------------------------------------------------------------