Showing preview only (4,753K chars total). Download the full file or copy to clipboard to get everything.
Repository: cambel/ur3
Branch: noetic-devel
Commit: 8b0c9dd857d2
Files: 233
Total size: 40.7 MB
Directory structure:
gitextract_k89eecrd/
├── .gitignore
├── .travis.yml
├── .vscode/
│ └── settings.json
├── BUILD-DOCKER-IMAGE.sh
├── LICENSE
├── README.md
├── RUN-DOCKER.sh
├── dependencies.rosinstall
├── docker/
│ ├── Dockerfile
│ ├── bashrc
│ ├── build_container.sh
│ ├── docker-compose.yml
│ └── launch_container.sh
├── ur.code-workspace
├── ur_control/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── gripper_controller.yaml
│ │ ├── ur_controllers.yaml
│ │ └── ur_e_controllers.yaml
│ ├── launch/
│ │ ├── ur_controllers.launch
│ │ └── ur_e_controllers.launch
│ ├── package.xml
│ ├── scripts/
│ │ ├── cartesian_compliance_controller_examples.py
│ │ ├── compliance_controller_examples.py
│ │ ├── controller_examples.py
│ │ ├── ft_filter.py
│ │ ├── getch.py
│ │ ├── imu.py
│ │ ├── joint_position_keyboard.py
│ │ ├── joint_position_mouse6d.py
│ │ ├── moveit_tutorial.py
│ │ └── wrench_republish.py
│ ├── setup.py
│ ├── src/
│ │ └── ur_control/
│ │ ├── __init__.py
│ │ ├── arm.py
│ │ ├── compliance_controller.py
│ │ ├── constants.py
│ │ ├── controllers.py
│ │ ├── controllers_connection.py
│ │ ├── conversions.py
│ │ ├── exceptions.py
│ │ ├── filters.py
│ │ ├── fzi_cartesian_compliance_controller.py
│ │ ├── grippers.py
│ │ ├── hybrid_controller.py
│ │ ├── impedance_control.py
│ │ ├── math_utils.py
│ │ ├── mouse_6d.py
│ │ ├── spalg.py
│ │ ├── traj_utils.py
│ │ ├── transformations.py
│ │ ├── ur_services.py
│ │ └── utils.py
│ └── test/
│ ├── test_quaternion_functions.py
│ └── transformations_bk.py
├── ur_gripper_85_moveit_config/
│ ├── .setup_assistant
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── cartesian_limits.yaml
│ │ ├── chomp_planning.yaml
│ │ ├── fake_controllers.yaml
│ │ ├── joint_limits.yaml
│ │ ├── kinematics.yaml
│ │ ├── ompl_planning.yaml
│ │ ├── ros_controllers.yaml
│ │ ├── sensors_3d.yaml
│ │ └── ur_robot_gazebo.srdf
│ ├── launch/
│ │ ├── chomp_planning_pipeline.launch.xml
│ │ ├── default_warehouse_db.launch
│ │ ├── demo.launch
│ │ ├── demo_gazebo.launch
│ │ ├── fake_moveit_controller_manager.launch.xml
│ │ ├── gazebo.launch
│ │ ├── gazebo_static_tf.launch
│ │ ├── joystick_control.launch
│ │ ├── move_group.launch
│ │ ├── moveit.rviz
│ │ ├── moveit_rviz.launch
│ │ ├── ompl_planning_pipeline.launch.xml
│ │ ├── pilz_industrial_motion_planner_planning_pipeline.launch.xml
│ │ ├── planning_context.launch
│ │ ├── planning_pipeline.launch.xml
│ │ ├── ros_controllers.launch
│ │ ├── run_benchmark_ompl.launch
│ │ ├── sensor_manager.launch.xml
│ │ ├── setup_assistant.launch
│ │ ├── start_moveit.launch
│ │ ├── trajectory_execution.launch.xml
│ │ ├── ur_robot_gazebo_moveit_controller_manager.launch.xml
│ │ ├── ur_robot_gazebo_moveit_sensor_manager.launch.xml
│ │ ├── warehouse.launch
│ │ └── warehouse_settings.launch.xml
│ └── package.xml
├── ur_gripper_description/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── config.rviz
│ ├── launch/
│ │ ├── bringup_with_gripper_85.launch
│ │ ├── display_with_gripper_85.launch
│ │ ├── display_with_gripper_hande.launch
│ │ ├── load_ur_gripper_85.launch.xml
│ │ └── load_ur_gripper_hande.launch.xml
│ ├── package.xml
│ └── urdf/
│ ├── ur_gripper_85.xacro
│ └── ur_gripper_hande.xacro
├── ur_gripper_gazebo/
│ ├── CMakeLists.txt
│ ├── launch/
│ │ ├── gazebo_to_tf.launch
│ │ ├── inc/
│ │ │ ├── load_ur_gripper_85.launch.xml
│ │ │ └── load_ur_gripper_hande.launch.xml
│ │ ├── ur3_cubes_example.launch
│ │ ├── ur3e_cubes_example.launch
│ │ ├── ur3e_with_gripper.launch
│ │ ├── ur5_cubes_example.launch
│ │ ├── ur_gripper_85_cubes.launch
│ │ └── ur_gripper_hande_cubes.launch
│ ├── models/
│ │ ├── floor/
│ │ │ ├── materials/
│ │ │ │ └── scripts/
│ │ │ │ └── checkboard.material
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── green_table/
│ │ │ ├── materials/
│ │ │ │ └── scripts/
│ │ │ │ └── repeated.material
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── multi_peg_board/
│ │ │ ├── meshes/
│ │ │ │ └── board.dae
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── peg_board/
│ │ │ ├── meshes/
│ │ │ │ └── board.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── simple_peg_board/
│ │ │ ├── meshes/
│ │ │ │ ├── simple_board.dae
│ │ │ │ └── simple_board.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur3_base/
│ │ │ ├── meshes/
│ │ │ │ ├── dualbase-big.dae
│ │ │ │ ├── dualbase.dae
│ │ │ │ └── ur3_base.stl
│ │ │ ├── model-bunri.sdf
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur3_table/
│ │ │ ├── meshes/
│ │ │ │ └── ur3_table.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur3e_base/
│ │ │ ├── materials/
│ │ │ │ └── scripts/
│ │ │ │ └── repeated.material
│ │ │ ├── meshes/
│ │ │ │ ├── ur3e_base.dae
│ │ │ │ ├── ur3e_base.stl
│ │ │ │ └── ur3e_base_table.stl
│ │ │ ├── model.config
│ │ │ ├── model.sdf
│ │ │ └── model_base.sdf
│ │ ├── ur3e_table/
│ │ │ ├── meshes/
│ │ │ │ └── ur3e_table.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur_base/
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── visual_marker/
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── wood_cube/
│ │ │ ├── model.config
│ │ │ ├── model.rsdf
│ │ │ └── model.sdf
│ │ └── wooden_peg/
│ │ ├── model.config
│ │ ├── model.rsdf
│ │ └── model.sdf
│ ├── package.xml
│ ├── scripts/
│ │ ├── gazebo_to_tf.py
│ │ ├── spawner.py
│ │ └── world_publisher.py
│ ├── setup.py
│ ├── src/
│ │ └── ur_gazebo/
│ │ ├── __init__.py
│ │ ├── basic_models.py
│ │ ├── gazebo_spawner.py
│ │ └── model.py
│ ├── urdf/
│ │ ├── ur_gripper_85.xacro
│ │ └── ur_gripper_hande.xacro
│ └── worlds/
│ ├── cubes_task.world
│ └── ur3e.world
├── ur_hande_moveit_config/
│ ├── .setup_assistant
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── cartesian_limits.yaml
│ │ ├── chomp_planning.yaml
│ │ ├── fake_controllers.yaml
│ │ ├── joint_limits.yaml
│ │ ├── kinematics.yaml
│ │ ├── ompl_planning.yaml
│ │ ├── ros_controllers.yaml
│ │ ├── sensors_3d.yaml
│ │ ├── ur3e.urdf
│ │ ├── ur3e_hande.urdf
│ │ └── ur_robot_gazebo.srdf
│ ├── launch/
│ │ ├── chomp_planning_pipeline.launch.xml
│ │ ├── default_warehouse_db.launch
│ │ ├── demo.launch
│ │ ├── demo_gazebo.launch
│ │ ├── fake_moveit_controller_manager.launch.xml
│ │ ├── gazebo.launch
│ │ ├── gazebo_static_tf.launch
│ │ ├── joystick_control.launch
│ │ ├── move_group.launch
│ │ ├── moveit.rviz
│ │ ├── moveit_rviz.launch
│ │ ├── ompl_planning_pipeline.launch.xml
│ │ ├── pilz_industrial_motion_planner_planning_pipeline.launch.xml
│ │ ├── planning_context.launch
│ │ ├── planning_pipeline.launch.xml
│ │ ├── ros_controllers.launch
│ │ ├── run_benchmark_ompl.launch
│ │ ├── sensor_manager.launch.xml
│ │ ├── setup_assistant.launch
│ │ ├── start_moveit.launch
│ │ ├── trajectory_execution.launch.xml
│ │ ├── ur_robot_gazebo_moveit_controller_manager.launch.xml
│ │ ├── ur_robot_gazebo_moveit_sensor_manager.launch.xml
│ │ ├── warehouse.launch
│ │ └── warehouse_settings.launch.xml
│ └── package.xml
├── ur_handeye_calibration/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── calibration_data_apriltag.npy
│ │ ├── settings.yaml
│ │ └── tags.yaml
│ ├── launch/
│ │ ├── apriltag_detect.launch
│ │ ├── aruco_detect.launch
│ │ ├── camera.launch
│ │ └── ur3e_fixed_handeye_calibration.launch
│ ├── package.xml
│ └── scripts/
│ ├── calibrate.py
│ ├── calibrator.py
│ └── capture_camera_poses.py
└── ur_pykdl/
├── CMakeLists.txt
├── LICENSE
├── package.xml
├── scripts/
│ ├── display_urdf.py
│ └── ur_kinematics.py
├── setup.py
├── src/
│ ├── ur_kdl/
│ │ ├── __init__.py
│ │ ├── kdl_kinematics.py
│ │ └── kdl_parser.py
│ └── ur_pykdl/
│ ├── __init__.py
│ └── ur_pykdl.py
└── urdf/
├── ur3.urdf
└── ur3e.urdf
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
*.py[cod]
# C extensions
*.so
# Packages
*.egg
*.egg-info
dist
build
eggs
parts
bin
var
sdist
develop-eggs
.installed.cfg
lib
lib64
# Installer logs
pip-log.txt
# Unit test / coverage reports
.coverage
.tox
nosetests.xml
# Translations
*.mo
# Mr Developer
.mr.developer.cfg
.project
.cproject
cmake_install.cmake
.pydevproject
*.swp
CATKIN_IGNORE
catkin
catkin_generated
devel
cpp
docs
msg_gen
srv_gen
*.smoketest
*.cfgc
*_msgs/src/
*/src/**/cfg
*/*/src/**/*
*.pyc
================================================
FILE: .travis.yml
================================================
# Generic .travis.yml file for running continuous integration on Travis-CI for
# any ROS package.
#
# Available here:
# - http://felixduvallet.github.io/ros-travis-integration
# - https://github.com/felixduvallet/ros-travis-integration
#
# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
# workspace, resolves all listed dependencies, and sets environment variables
# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
# no compilation errors), and runs all the tests. If any of the compilation/test
# phases fail, the build is marked as a failure.
#
# We handle two types of package dependencies specified in the package manifest:
# - system dependencies that can be installed using `rosdep`, including other
# ROS packages and system libraries. These dependencies must be known to
# `rosdistro` and get installed using apt-get.
# - package dependencies that must be checked out from source. These are handled by
# `wstool`, and should be listed in a file named dependencies.rosinstall.
#
# There are two variables you may want to change:
# - ROS_DISTRO (default is indigo). Note that packages must be available for
# ubuntu 14.04 trusty.
# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
# root). This should list all necessary repositories in wstool format (see
# the ros wiki). If the file does not exists then nothing happens.
#
# See the README.md for more information.
#
# Author: Felix Duvallet <felixd@gmail.com>
# NOTE: The build lifecycle on Travis.ci is something like this:
# before_install
# install
# before_script
# script
# after_success or after_failure
# after_script
# OPTIONAL before_deploy
# OPTIONAL deploy
# OPTIONAL after_deploy
################################################################################
# Use ubuntu trusty (14.04) with sudo privileges.
dist: trusty
sudo: required
language:
- generic
cache:
- apt
# Configuration variables. All variables are global now, but this can be used to
# trigger a build matrix for different ROS distributions if desired.
env:
global:
- ROS_DISTRO=indigo
- ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...]
- CI_SOURCE_PATH=$(pwd)
- ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
- CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
- ROS_PARALLEL_JOBS='-j8 -l6'
# Set the python path manually to include /usr/-/python2.7/dist-packages
# as this is where apt-get installs python packages.
- PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages
################################################################################
# Install system dependencies, namely a very barebones ROS setup.
before_install:
- sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
- sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
- sudo apt-get update -qq
- sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
- source /opt/ros/$ROS_DISTRO/setup.bash
# Prepare rosdep to install dependencies.
- sudo rosdep init
- rosdep update
# Create a catkin workspace with the package under integration.
install:
- mkdir -p ~/catkin_ws/src
- cd ~/catkin_ws/src
- catkin_init_workspace
# Create the devel/setup.bash (run catkin_make with an empty workspace) and
# source it to set the path variables.
- cd ~/catkin_ws
- catkin_make
- source devel/setup.bash
# Add the package under integration to the workspace using a symlink.
- cd ~/catkin_ws/src
- ln -s $CI_SOURCE_PATH .
# Install all dependencies, using wstool first and rosdep second.
# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
before_script:
# source dependencies: install using wstool.
- cd ~/catkin_ws/src
- wstool init
- if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
- wstool up
# package depdencies: install using rosdep.
- cd ~/catkin_ws
- rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
# Compile and test (mark the build as failed if any step fails). If the
# CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example
# to blacklist certain packages.
#
# NOTE on testing: `catkin_make run_tests` will show the output of the tests
# (gtest, nosetest, etc..) but always returns 0 (success) even if a test
# fails. Running `catkin_test_results` aggregates all the results and returns
# non-zero when a test fails (which notifies Travis the build failed).
script:
- source /opt/ros/$ROS_DISTRO/setup.bash
- cd ~/catkin_ws
- catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
# Run the tests, ensuring the path is set correctly.
- source devel/setup.bash
- catkin_make run_tests && catkin_test_results
================================================
FILE: .vscode/settings.json
================================================
{
"python.linting.pylintEnabled": true,
"python.linting.enabled": true
}
================================================
FILE: BUILD-DOCKER-IMAGE.sh
================================================
#!/bin/bash
# Generates a docker image with the relevant settings for the DOCKER_PROJECT.
# Context-sensitive behaviour: If the <user> parameter "gitlab-ci" is used,
# the script builds the image without trying to download it.
#
# Usage: ./BUILD-DOCKER-IMAGE.bash <optional: user>
#
# @param <user> [optional parameter] for docker container naming during spin-up and to set different behavior.
# Default value: $USER - the image is pulled from repo, or built as fallback.
# If <user> is "gitlab-ci" the image is directly build from scratch - as if done in gitlab-ci.
################################################################################
# Set the Docker container name from a project name (first argument).
# If no argument is given, use the current user name as the project name.
DOCKER_PROJECT=$1
if [ -z "${DOCKER_PROJECT}" ]; then
DOCKER_PROJECT=${USER}
fi
DOCKER_CONTAINER="${DOCKER_PROJECT}_ros_ur_1"
echo "$0: DOCKER_PROJECT=${DOCKER_PROJECT}"
echo "$0: DOCKER_CONTAINER=${DOCKER_CONTAINER}"
# Stop and remove the Docker container.
EXISTING_DOCKER_CONTAINER_ID=`docker ps -aq -f name=${DOCKER_CONTAINER}`
if [ ! -z "${EXISTING_DOCKER_CONTAINER_ID}" ]; then
echo "Stop the container ${DOCKER_CONTAINER} with ID: ${EXISTING_DOCKER_CONTAINER_ID}."
docker stop ${EXISTING_DOCKER_CONTAINER_ID}
echo "Remove the container ${DOCKER_CONTAINER} with ID: ${EXISTING_DOCKER_CONTAINER_ID}."
docker rm ${EXISTING_DOCKER_CONTAINER_ID}
fi
docker-compose -p ${DOCKER_PROJECT} -f ./docker/docker-compose.yml build
================================================
FILE: LICENSE
================================================
The MIT License (MIT)
Copyright (c) 2018-2023 Cristian Beltran
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
Universal Robot UR/URe
===
<img src="https://github.com/cambel/ur3/blob/noetic-devel/wiki/ur3e.gif?raw=true" alt="UR3e & Robotiq Hand-e" width="250"><img src="https://github.com/cambel/ur3/blob/noetic-devel/wiki/ur3.gif?raw=true" alt="UR3 & Robotiq 85" width="250">
Custom ROS packages for the UR3 Robot with a gripper Robotiq 85 and the UR3e robot with a gripper Robotiq Hand-e.
Tested on ROS Noetic Ubuntu 20.04 with Python 3.8.
For ROS Melodic see the `melodic-devel` branch.
## Installation
### [With docker](https://github.com/cambel/ur3/wiki/Install-with-Docker)
### [Compile from source](https://github.com/cambel/ur3/wiki/Compile-from-source)
## Examples
### Visualization of Universal Robot in RViz
To visualize the model of the robot with a gripper, launch the following:
```
roslaunch ur_gripper_description display_with_gripper_hande.launch ur_robot:=ur5e
```
You can then use the sliders to change the joint values and the gripper values.
Change the value of ur_robot to any other valid robot (ur3e, ur5e, ...)
### Simulation in Gazebo 9
<img src="https://github.com/cambel/ur3/blob/noetic-devel/wiki/ur3-e.png?raw=true" width="500">
To simulate the robot launch the following:
```
roslaunch ur_gripper_gazebo ur_gripper_85_cubes.launch ur_robot:=ur3 grasp_plugin:=1
```
or using ur3e:
```
roslaunch ur_gripper_gazebo ur_gripper_hande_cubes.launch ur_robot:=ur3e grasp_plugin:=1
```
You can then send commands to the joints or to the gripper.
An example of sending joints values to the robot can be executed as follows:
```
rosrun ur_control sim_controller_examples.py -m
```
To change the values of the joints, the file `sim_controller_examples.py` must be modified.
Similarly, the script include examples to control the robot's end-effector position, gripper and an example of performing grasping.
Execute the following command to see the available examples.
```
rosrun ur_control sim_controller_examples.py --help
```
For testing the grasping examples you need to explicitly specify that the gripper is going to be loaded, e.g.,
```
rosrun ur_control sim_controller_examples.py --gripper --grasp_naive
```
The grasp_plugin example uses this [plugin](https://github.com/pal-robotics/gazebo_ros_link_attacher), and requires gazebo to be launched with the grasp_plugin parameter as `True`.
An easy way to control the robot using the keyboard can be found in the script:
```
rosrun ur_control joint_position_keyboard.py
```
Press SPACE to get a list of all valid commands to control either each independent joint or the end effector position x,y,z and rotations.
To have access to the gripper controller include the option `--gripper`
Another option of easy control is using `rqt`
## MoveIt
To test the MoveIt configuration with any UR/URe robot, start one of the gazebo environments, such as:
```
roslaunch ur_gripper_gazebo ur_gripper_hande_cubes.launch ur_robot:=ur3e grasp_plugin:=1
```
Then load the MoveIt configuration
```
roslaunch ur_hande_moveit_config start_moveit.launch
```
Then execute the tutorial
```
rosrun ur_control moveit_tutorial.py --tutorial
```
================================================
FILE: RUN-DOCKER.sh
================================================
#!/bin/bash
################################################################################
export DOCKER_RUNTIME=nvidia
# Set the Docker container name from a project name (first argument).
# If no argument is given, use the current user name as the project name.
PROJECT=$1
if [ -z "${PROJECT}" ]; then
PROJECT=${USER}
fi
CONTAINER="${PROJECT}_ros_ur_1"
echo "$0: PROJECT=${PROJECT}"
echo "$0: CONTAINER=${CONTAINER}"
# Run the Docker container in the background.
# Any changes made to './docker/docker-compose.yml' will recreate and overwrite the container.
docker-compose -p ${PROJECT} -f ./docker/docker-compose.yml up -d
################################################################################
# Display GUI through X Server by granting full access to any external client.
xhost +
################################################################################
# Enter the Docker container with a Bash shell (with or without a custom).
docker exec -it ${CONTAINER} bash
================================================
FILE: dependencies.rosinstall
================================================
- git:
local-name: robotiq
uri: https://github.com/cambel/robotiq.git
version: noetic-devel
- git:
local-name: gazebo_ros_link_attacher
uri: https://github.com/pal-robotics/gazebo_ros_link_attacher.git
version: melodic-devel
- git:
local-name: robotiq_urcap_control
uri: https://github.com/cambel/ros_robotiq_urcap_control.git
version: master
- git:
local-name: Universal_Robots_ROS_Driver
uri: https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git
version: master
- git:
local-name: trac_ik
uri: https://bitbucket.org/traclabs/trac_ik.git
version: master
- git:
local-name: universal_robot
uri: https://github.com/fmauch/universal_robot.git
version: calibration_devel
- git:
local-name: ur_msgs
uri: https://github.com/ros-industrial/ur_msgs.git
version: melodic-devel
================================================
FILE: docker/Dockerfile
================================================
# Docker for ur3 repo
# ros-noetic-base, gazebo11, python3 libraries
# Python3 3 version
FROM osrf/ros:noetic-desktop-full
LABEL maintainer Cristian Beltran "beltran@hlab.sys.es.osaka-u.ac.jp"
ENV ROS_DISTRO noetic
# install universal robot ros packages
RUN apt-get update && apt-get install -y \
# ROS utils
ros-$ROS_DISTRO-gazebo-ros-pkgs \
ros-$ROS_DISTRO-spacenav-node \
ros-$ROS_DISTRO-rqt-common-plugins \
ros-$ROS_DISTRO-rqt-joint-trajectory-controller \
ros-$ROS_DISTRO-rqt-tf-tree \
ros-$ROS_DISTRO-rqt-multiplot \
ros-$ROS_DISTRO-moveit \
# install catkin
ros-$ROS_DISTRO-catkin \
python3-catkin-tools \
# utils
git \
locate \
aptitude \
vim htop \
curl wget \
spacenavd \
python-is-python3 \
python3-pip \
&& rm -rf /var/lib/apt/lists/*
### Ros Workspace ###
# Set up the workspace
RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash \
&& mkdir -p ~/ros_ws/src \
&& cd ~/ros_ws/src \
&& git clone https://github.com/cambel/ur3.git ros-ur -b noetic-devel"
# Updating ROSDEP and installing dependencies
RUN cd ~/ros_ws \
&& rosinstall ~/ros_ws/src /opt/ros/$ROS_DISTRO src/ros-ur/dependencies.rosinstall \
&& apt-get update \
&& rosdep fix-permissions \
&& rosdep update \
&& rosdep install --from-paths src --ignore-src --rosdistro=$ROS_DISTRO -y \
&& rm -rf /var/lib/apt/lists/*
ENV PYTHONIOENCODING UTF-8
## Python3 libraries ##
RUN python3 -m pip install pip --upgrade && \
pip install matplotlib==2.2.3 spicy protobuf pyyaml pyquaternion rospkg \
lxml tqdm catkin-pkg empy PyVirtualDisplay defusedxml gym psutil pyprind
# # Compiling ros workspace
RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash \
&& cd ~/ros_ws/ \
&& rm -rf build \
&& catkin build"
################################################
# Custom python3 libs
################################################
# ur_ikfast
# RUN /bin/bash -c "cd ~/ \
# && mkdir pylibs && cd pylibs \
# && git clone https://github.com/cambel/ur_ikfast.git \
# && cd ur_ikfast && pip install -e ."
# Download gazebo models only once
RUN mkdir -p ~/.gazebo
RUN git clone https://github.com/osrf/gazebo_models ~/.gazebo/models
ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all}
ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
# setup environment
EXPOSE 11345
RUN echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> ~/.bashrc
RUN echo 'source ~/ros_ws/devel/setup.bash' >> ~/.bashrc
RUN echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc
RUN echo 'export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:~/ros_ws/src/ros-universal-robots/ur_gripper_gazebo/models/' >> ~/.bashrc
================================================
FILE: docker/bashrc
================================================
================================================
FILE: docker/build_container.sh
================================================
#!/bin/bash
# Generates a docker image with the relevant settings for the DOCKER_PROJECT.
# Context-sensitive behaviour: If the <user> parameter "gitlab-ci" is used,
# the script builds the image without trying to download it.
#
# Usage: ./BUILD-DOCKER-IMAGE.bash <optional: user>
#
# @param <user> [optional parameter] for docker container naming during spin-up and to set different behavior.
# Default value: $USER - the image is pulled from repo, or built as fallback.
# If <user> is "gitlab-ci" the image is directly build from scratch - as if done in gitlab-ci.
################################################################################
# Set the Docker container name from a project name (first argument).
# If no argument is given, use the current user name as the project name.
DOCKER_PROJECT=$1
if [ -z "${DOCKER_PROJECT}" ]; then
DOCKER_PROJECT=${USER}
fi
DOCKER_CONTAINER="${PROJECT}-ros_ur3-1"
echo "$0: DOCKER_PROJECT=${DOCKER_PROJECT}"
echo "$0: DOCKER_CONTAINER=${DOCKER_CONTAINER}"
# Stop and remove the Docker container.
EXISTING_DOCKER_CONTAINER_ID=`docker ps -aq -f name=${DOCKER_CONTAINER}`
if [ ! -z "${EXISTING_DOCKER_CONTAINER_ID}" ]; then
echo "Stop the container ${DOCKER_CONTAINER} with ID: ${EXISTING_DOCKER_CONTAINER_ID}."
docker stop ${EXISTING_DOCKER_CONTAINER_ID}
echo "Remove the container ${DOCKER_CONTAINER} with ID: ${EXISTING_DOCKER_CONTAINER_ID}."
docker rm ${EXISTING_DOCKER_CONTAINER_ID}
fi
docker compose -p ${DOCKER_PROJECT} -f ./docker-compose.yml build
================================================
FILE: docker/docker-compose.yml
================================================
version: "2.4"
################################################################################
services:
ros_ur:
hostname: localhost
runtime: ${DOCKER_RUNTIME}
privileged: true
working_dir: /root/
environment:
# Pass host user information.
- HOST_USER=${USER}
- HOST_UID=1000
- HOST_GID=1000
# Display X Server GUI.
- DISPLAY
- QT_X11_NO_MITSHM=1
# Configure Nvidia Docker interface.
- NVIDIA_VISIBLE_DEVICES=all
- NVIDIA_DRIVER_CAPABILITIES=all
- NVIDIA_REQUIRE_CUDA=cuda>=9.0
- DOCKER_RUNTIME=${DOCKER_RUNTIME}
# Needs to be explicitly define
- ROS_HOSTNAME=localhost
image: ros-ur:noetic
tty: true
build:
context: ../
dockerfile: ./docker/Dockerfile
args:
- DOCKERFILE_COMMIT_SHORT_SHA
volumes:
# Map ROS workspace folders.
- ../:/root/ros_ws/src/ros-ur
# Grant display access to X Server.
- /tmp/.X11-unix:/tmp/.X11-unix
# Grant bus access to FlexBE App.
- /var/run/dbus/system_bus_socket:/var/run/dbus/system_bus_socket
# Grant access to serial usb ports for u2d2
- /dev/:/dev/
network_mode: host
command: tail -f /dev/null
================================================
FILE: docker/launch_container.sh
================================================
#!/bin/bash
################################################################################
# Set the Docker container name from a project name (first argument).
# If no argument is given, use the current user name as the project name.
PROJECT=$1
if [ -z "${PROJECT}" ]; then
PROJECT=${USER}
fi
CONTAINER="${PROJECT}-ros_ur3-1"
echo "$0: PROJECT=${PROJECT}"
echo "$0: CONTAINER=${CONTAINER}"
# Run the Docker container in the background.
# Any changes made to './docker/docker-compose.yml' will recreate and overwrite the container.
docker compose -p ${PROJECT} -f ./docker-compose.yml up -d
################################################################################
# Display GUI through X Server by granting full access to any external client.
xhost +
################################################################################
# Enter the Docker container with a Bash shell (with or without a custom).
docker exec -it ${CONTAINER} bash
================================================
FILE: ur.code-workspace
================================================
{
"folders": [
{
"path": "."
},
],
"settings": {
"files.insertFinalNewline": true,
"files.exclude": {
"**/**.pyc": true,
"**/build": true,
"**/devel": true,
"**/logs": true,
"**/.catkin_tools": true,
},
"cSpell.words": [
"interp",
"mtype",
"openai",
"pyquaternion",
"rospy",
"spalg"
],
"python.analysis.extraPaths": [
"./ur_control/src",
"./ur_gripper_gazebo/src",
"./ur3e_openai/src",
"./ur3e_rl/src",
],
"python.analysis.autoImportCompletions": true,
"python.autoComplete.extraPaths": [
"/root/ros_ws/devel/lib/python3/dist-packages",
"/opt/ros/noetic/lib/python3/dist-packages",
],
"[python]": {
"editor.defaultFormatter": "ms-python.autopep8"
},
"autopep8.args": [
"--max-line-length",
"200",
"--experimental"
],
}
}
================================================
FILE: ur_control/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.1.3)
project(ur_control)
find_package(catkin REQUIRED COMPONENTS
control_msgs
controller_manager
effort_controllers
gripper_action_controller
joint_state_controller
joint_trajectory_controller
std_srvs
geometry_msgs
robot_state_publisher
sensor_msgs
trajectory_msgs
trac_ik_python
)
catkin_python_setup()
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
================================================
FILE: ur_control/config/gripper_controller.yaml
================================================
# Gripper controller
gripper_controller:
type: position_controllers/GripperActionController
gripper_type: '85'
joint: finger_joint
action_monitor_rate: 20
goal_tolerance: 0.002
max_effort: 100
stall_velocity_threshold: 0.001
stall_timeout: 1.0
================================================
FILE: ur_control/config/ur_controllers.yaml
================================================
# Joint state controller
joint_state_controller:
publish_rate: 125
type: joint_state_controller/JointStateController
scaled_pos_joint_traj_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
# Default is 0
goal_time: 0.1
# Default is 0.01
stopped_velocity_tolerance: 0.01
shoulder_pan_joint:
trajectory: 0
goal: 0.01
shoulder_lift_joint:
trajectory: 0
goal: 0.01
elbow_joint:
trajectory: 0
goal: 0.01
wrist_1_joint:
trajectory: 0
goal: 0.01
wrist_2_joint:
trajectory: 0
goal: 0.01
wrist_3_joint:
trajectory: 0
goal: 0.01
#Position controller
shoulder_pan_joint:
type: position_controllers/JointPositionController
joint: shoulder_pan_joint
shoulder_lift_joint:
type: position_controllers/JointPositionController
joint: shoulder_lift_joint
elbow_joint:
type: position_controllers/JointPositionController
joint: elbow_joint
wrist_1_joint:
type: position_controllers/JointPositionController
joint: wrist_1_joint
wrist_2_joint:
type: position_controllers/JointPositionController
joint: wrist_2_joint
wrist_3_joint:
type: position_controllers/JointPositionController
joint: wrist_3_joint
================================================
FILE: ur_control/config/ur_e_controllers.yaml
================================================
# Joint state controller
joint_state_controller:
publish_rate: 500
type: joint_state_controller/JointStateController
scaled_pos_joint_traj_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
# Default is 0
goal_time: 0.1
# Default is 0.01
stopped_velocity_tolerance: 0.01
shoulder_pan_joint:
trajectory: 0
goal: 0.01
shoulder_lift_joint:
trajectory: 0
goal: 0.01
elbow_joint:
trajectory: 0
goal: 0.01
wrist_1_joint:
trajectory: 0
goal: 0.01
wrist_2_joint:
trajectory: 0
goal: 0.01
wrist_3_joint:
trajectory: 0
goal: 0.01
#Position controller
shoulder_pan_joint:
type: position_controllers/JointPositionController
joint: shoulder_pan_joint
shoulder_lift_joint:
type: position_controllers/JointPositionController
joint: shoulder_lift_joint
elbow_joint:
type: position_controllers/JointPositionController
joint: elbow_joint
wrist_1_joint:
type: position_controllers/JointPositionController
joint: wrist_1_joint
wrist_2_joint:
type: position_controllers/JointPositionController
joint: wrist_2_joint
wrist_3_joint:
type: position_controllers/JointPositionController
joint: wrist_3_joint
================================================
FILE: ur_control/launch/ur_controllers.launch
================================================
<?xml version="1.0"?>
<launch>
<!-- Launch file parameters -->
<arg name="debug" default="true" />
<arg if= "$(arg debug)" name="DEBUG" value="screen"/>
<arg unless = "$(arg debug)" name="DEBUG" value="log"/>
<arg name="gripper_robotiq_85" default="false" />
<arg name="gripper_robotiq_hande" default="false" />
<arg if="$(eval gripper_robotiq_85 or gripper_robotiq_hande)" name="gripper_controller" value="gripper_controller"/>
<arg unless="$(eval gripper_robotiq_85 or gripper_robotiq_hande)" name="gripper_controller" value=""/>
<!-- Controllers config -->
<rosparam file="$(find ur_control)/config/ur_controllers.yaml" command="load" />
<group if="$(arg gripper_robotiq_85)">
<rosparam file="$(find robotiq_control)/config/gripper_85_controller.yaml" command="load" />
</group>
<group if="$(arg gripper_robotiq_hande)">
<rosparam file="$(find robotiq_control)/config/gripper_hande_controller.yaml" command="load" />
</group>
<!-- Load controllers -->
<node name="robot_controllers" pkg="controller_manager" type="spawner" respawn="false"
output="$(arg DEBUG)"
args="joint_state_controller scaled_pos_joint_traj_controller gripper_controller"/>
<!-- TF -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50" />
</node>
</launch>
================================================
FILE: ur_control/launch/ur_e_controllers.launch
================================================
<?xml version="1.0"?>
<launch>
<!-- Launch file parameters -->
<arg name="debug" default="true" />
<arg name="gripper_robotiq_85" default="false" />
<arg name="gripper_robotiq_hande" default="false" />
<arg if="$(eval gripper_robotiq_85 or gripper_robotiq_hande)" name="gripper_controller" value="gripper_controller"/>
<arg unless="$(eval gripper_robotiq_85 or gripper_robotiq_hande)" name="gripper_controller" value=""/>
<arg if= "$(arg debug)" name="DEBUG" value="screen"/>
<arg unless = "$(arg debug)" name="DEBUG" value="log"/>
<!-- Controllers config -->
<rosparam file="$(find ur_control)/config/ur_e_controllers.yaml" command="load"/>
<group if="$(arg gripper_robotiq_85)">
<rosparam file="$(find ur_control)/config/gripper_85_controller.yaml" command="load" />
</group>
<group if="$(arg gripper_robotiq_hande)">
<rosparam file="$(find robotiq_control)/config/gripper_hande_controller.yaml" command="load" />
</group>
<!-- stuff that will only be evaluated if foo is true -->
<!-- Load controllers -->
<node name="robot_controllers" pkg="controller_manager" type="spawner" respawn="false"
output="$(arg DEBUG)"
args="joint_state_controller scaled_pos_joint_traj_controller $(arg gripper_controller)"/>
<!-- TF -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="state_publisher">
<param name="publish_frequency" type="double" value="500" />
</node>
</launch>
================================================
FILE: ur_control/package.xml
================================================
<?xml version="1.0"?>
<package>
<name>ur_control</name>
<version>0.1.0</version>
<description>The ur_control package</description>
<author email="cristianbehe@gmail.com">Cristian Beltran</author>
<maintainer email="cristianbehe@gmail.com">Cristian Beltran</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>control_msgs</build_depend>
<build_depend>controller_manager</build_depend>
<build_depend>effort_controllers</build_depend>
<build_depend>gripper_action_controller</build_depend>
<build_depend>joint_state_controller</build_depend>
<build_depend>joint_trajectory_controller</build_depend>
<build_depend>robot_state_publisher</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>trac_ik_python</build_depend>
<run_depend>control_msgs</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>effort_controllers</run_depend>
<run_depend>gripper_action_controller</run_depend>
<run_depend>joint_state_controller</run_depend>
<run_depend>joint_trajectory_controller</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>trac_ik_python</run_depend>
</package>
================================================
FILE: ur_control/scripts/cartesian_compliance_controller_examples.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2023 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import sys
import signal
from ur_control import spalg, utils, traj_utils, constants
from ur_control.fzi_cartesian_compliance_controller import CompliantController
import argparse
import rospy
import numpy as np
np.set_printoptions(suppress=True)
np.set_printoptions(linewidth=np.inf)
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
def move_joints():
q = [1.3506, -1.6493, 1.9597, -1.8814, -1.5652, 1.3323]
q = [1.4414, -1.7303, 2.145, -1.9863, -1.5656, 1.4397]
q = [1.0497, -1.5863, 2.0175, -2.005, -1.5681, 1.0542]
q = [1.4817, -2.0874, 1.7722, -1.2554, -1.5669, 0.0189]
arm.set_joint_positions(positions=q, target_time=3, wait=True)
def move_cartesian():
q = [1.3524, -1.5555, 1.7697, -1.7785, -1.5644, 1.3493]
arm.set_joint_positions(positions=q, target_time=3, wait=True)
# arm.set_position_control_mode(False)
# arm.set_control_mode(mode="spring-mass-damper")
arm.set_position_control_mode(True)
arm.set_control_mode(mode="parallel")
arm.set_solver_parameters(error_scale=0.5, iterations=1)
arm.update_stiffness([1500, 1500, 1500, 100, 100, 100])
# selection_matrix = [0.5, 0.5, 1, 0.5, 0.5, 0.5]
selection_matrix = np.ones(6)
arm.update_selection_matrix(selection_matrix)
p_gains = [0.05, 0.05, 0.05, 1.5, 1.5, 1.5]
d_gains = [0.005, 0.005, 0.005, 0, 0, 0]
arm.update_pd_gains(p_gains, d_gains=d_gains)
ee = arm.end_effector()
p1 = ee.copy()
p1[2] -= 0.03
p2 = p1.copy()
p2[2] += 0.005
trajectory = p1
# trajectory = np.stack((p1, p2))
target_force = np.zeros(6)
def f(x): return rospy.loginfo_throttle(0.25, f"error: {np.round(trajectory[:3] - x[:3], 4)}")
arm.zero_ft_sensor()
res = arm.execute_compliance_control(trajectory, target_wrench=target_force, max_force_torque=[50., 50., 50., 5., 5., 5.],
duration=5, func=f, scale_up_error=True, max_scale_error=3.0, auto_stop=False)
print("EE total displacement", np.round(ee - arm.end_effector(), 4))
print("Pose error", np.round(trajectory[:3] - arm.end_effector()[:3], 4))
def move_force():
""" Linear push. Move until the target force is felt and stop. """
arm.zero_ft_sensor()
arm.set_control_mode("parallel")
selection_matrix = [1, 1, 0, 1, 1, 1]
arm.update_selection_matrix(selection_matrix)
# arm.set_control_mode("spring-mass-damper")
arm.set_solver_parameters(error_scale=0.5, iterations=1)
arm.update_stiffness([1500, 1500, 1500, 100, 100, 100])
p_gains = [0.05, 0.05, 0.1, 1.5, 1.5, 1.5]
d_gains = [0.005, 0.005, 0.005, 0, 0, 0]
arm.update_pd_gains(p_gains, d_gains)
ee = arm.end_effector()
target_force = [0, 0, -5, 0, 0, 0] # express in the end_effector_link
# account for the direction of the force?
stop_at_wrench = np.copy(target_force)
stop_at_wrench *= -1
# transform = arm.end_effector(tip_link="b_bot_tool0")
# tf = spalg.convert_wrench(target_force, transform)
# print(transform)
# print("TF", tf[:3])
res = arm.execute_compliance_control(ee, target_wrench=target_force,
max_force_torque=[30., 30., 30., 4., 4., 4.], duration=15,
stop_at_wrench=stop_at_wrench,
stop_on_target_force=True)
print(res)
print("EE change", ee - arm.end_effector())
def slicing():
""" Push down while oscillating in X-axis or Y-axis """
arm.zero_ft_sensor()
selection_matrix = [1, 1, 0, 1, 1, 1]
arm.update_selection_matrix(selection_matrix)
pd_gains = [0.03, 0.03, 0.03, 1.0, 1.0, 1.0]
arm.update_pd_gains(pd_gains)
ee = arm.end_effector()
trajectory = traj_utils.compute_sinusoidal_trajectory(ee, dimension=1, period=3, amplitude=0.02, num_of_points=100)
target_force = [0, 0, -3, 0, 0, 0] # express in the end_effector_link
res = arm.execute_compliance_control(trajectory, target_wrench=target_force,
max_force_torque=[50., 50., 50., 5., 5., 5.], duration=20)
print(res)
print("EE change", ee - arm.end_effector())
def admittance_control():
""" Spring-mass-damper force control """
rospy.loginfo("START ADMITTANCE")
arm.set_control_mode(mode="spring-mass-damper")
ee = arm.end_effector()
target_force = np.zeros(6)
arm.execute_compliance_control(ee, target_wrench=target_force,
max_force_torque=[50., 50., 50., 5., 5., 5.], duration=10,
stop_on_target_force=False)
rospy.loginfo("STOP ADMITTANCE")
def free_drive():
rospy.loginfo("START FREE DRIVE")
rospy.sleep(0.5)
arm.zero_ft_sensor()
# arm.set_control_mode("parallel")
arm.set_control_mode("spring-mass-damper")
# arm.set_hand_frame_control(False)
# arm.set_end_effector_link("b_bot_tool0")
# selection_matrix = [0., 0., 0., 1., 1., 1.]
# selection_matrix = [1., 1., 1., 0., 0., 0.]
# arm.update_selection_matrix(selection_matrix)
# 0.8 is vibrating
arm.set_solver_parameters(error_scale=1.0, iterations=1.0)
# pd_gains = [0.03, 0.03, 0.03, 1.0, 1.0, 1.0]
# pd_gains = [0.06, 0.06, 0.06, 1.5, 1.5, 1.5] # stiff
pd_gains = [0.1, 0.1, 0.1, 2.0, 2.0, 3.0] # flex
d_gains = [0.0, 0.0, 0.0, 0, 0, 0]
# d_gains = [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
# d_gains = [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] # good with p0.1 e_s0.8 ite1.0
# d_gains = [0.0001, 0.0001, 0.0001, 0, 0, 0]
# pd_gains = [0.08, 0.08, 0.08, 1.5, 1.5, 1.5]
arm.update_pd_gains(pd_gains, d_gains)
# arm.update_stiffness([10, 10, 10, 10, 10, 10])
# arm.update_stiffness([1000, 1000, 1000, 50, 50, 20])
# arm.update_stiffness([250, 250, 250, 5, 5, 5]) # 10 for pos can make it drift
arm.update_stiffness([100, 100, 100, 5, 5, 5]) # 10 for pos can make it drift
# mapping parameter space vs vibration
# how to reduce vibration...
# TODO: rotation is wrong!
# add wrench feedback
#
ee = arm.end_effector()
target_force = np.zeros(6)
target_force[1] += 0
res = arm.execute_compliance_control(ee, target_wrench=target_force,
max_force_torque=[50., 50., 50., 5., 5., 5.], duration=60,
stop_on_target_force=False)
print(res)
print("EE change", ee - arm.end_effector())
rospy.loginfo("STOP FREE DRIVE")
def test():
# start here
move_joints()
for _ in range(3):
# Move down (cut)
arm.move_relative(transformation=[0, 0, -0.03, 0, 0, 0], relative_to_tcp=False, duration=0.5, wait=True)
# Move back up and to the next initial pose
arm.move_relative(transformation=[0, 0, 0.03, 0, 0, 0], relative_to_tcp=False, duration=0.25, wait=True)
arm.move_relative(transformation=[0, 0.01, 0, 0, 0, 0], relative_to_tcp=False, duration=0.25, wait=True)
def enable_compliance_control():
q = [1.3524, -1.5555, 1.7697, -1.7785, -1.5644, 1.3493]
arm.set_joint_positions(q, t=1, wait=True)
arm.zero_ft_sensor()
# arm.set_control_mode(mode="parallel")
arm.set_control_mode(mode="spring-mass-damper")
# arm.set_position_control_mode(True)
# arm.update_selection_matrix(np.array([0.1]*6))
arm.activate_cartesian_controller()
arm.set_cartesian_target_pose(arm.end_effector(tip_link="b_bot_gripper_tip_link"))
for _ in range(30):
print("current target pose", arm.current_target_pose[:3])
print("error", arm.current_target_pose[:3] - arm.end_effector(tip_link="b_bot_gripper_tip_link")[:3])
rospy.sleep(1)
arm.activate_joint_trajectory_controller()
def main():
""" Main function to be run. """
parser = argparse.ArgumentParser(description='Test force control')
parser.add_argument('-m', '--move_joints', action='store_true',
help='move to joint configuration')
parser.add_argument('-mc', '--move_cartesian', action='store_true',
help='move to cartesian configuration')
parser.add_argument('-mf', '--move_force', action='store_true',
help='move towards target force')
parser.add_argument('-fd', '--free_drive', action='store_true',
help='move the robot freely')
parser.add_argument('-a', '--admittance', action='store_true',
help='Spring-mass-damper force control demo')
parser.add_argument('-s', '--slicing', action='store_true',
help='Push down while oscillating on X-axis')
parser.add_argument('-t', '--test', action='store_true',
help='Test')
parser.add_argument('-teleop', '--teleoperation', action='store_true',
help='Enable cartesian controllers for teleoperation')
parser.add_argument('--namespace', type=str,
help='Namespace of arm', default=None)
args = parser.parse_args()
rospy.init_node('ur3e_compliance_control')
ns = "None"
joints_prefix = None
tcp_link = 'gripper_tip_link'
# tcp_link = 'wrist_3_link'
# tcp_link = 'tool0'
if args.namespace:
ns = args.namespace
joints_prefix = args.namespace + '_'
global arm
arm = CompliantController(namespace=ns,
joint_names_prefix=joints_prefix,
ee_link=tcp_link,
ft_topic='wrench',
gripper_type=None)
arm.dashboard_services.activate_ros_control_on_ur()
if args.move_joints:
move_joints()
if args.move_cartesian:
move_cartesian()
if args.move_force:
move_force()
if args.admittance:
admittance_control()
if args.free_drive:
free_drive()
if args.slicing:
slicing()
if args.test:
test()
if args.teleoperation:
enable_compliance_control()
# if args.hand_frame_control:
# move_hand_frame_control()
if __name__ == "__main__":
main()
================================================
FILE: ur_control/scripts/compliance_controller_examples.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import sys
import signal
from ur_control import utils, traj_utils
from ur_control.hybrid_controller import ForcePositionController
from ur_control.compliance_controller import CompliantController
import argparse
import rospy
import numpy as np
np.set_printoptions(suppress=True)
np.set_printoptions(linewidth=np.inf)
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
def move_joints(wait=True):
# desired joint configuration 'q'
q = [1.57, -1.57, 1.26, -1.57, -1.57, 0]
# go to desired joint configuration
# in t time (seconds)
# wait is for waiting to finish the motion before executing
# anything else or ignore and continue with whatever is next
arm.set_joint_positions(positions=q, wait=wait, target_time=2.0)
def spiral_trajectory():
"""
Force/Position control. Follow a spiral trajectory on the world's YZ plan while controlling force on Z
"""
initial_q = [1.57, -1.57, 1.26, -1.57, -1.57, 0]
arm.set_joint_positions(positions=initial_q, wait=True, target_time=2)
plane = "YZ"
radius = 0.02
radius_direction = "+Z"
revolutions = 3
steps = 100 # Number of waypoints of the spiral trajectory
duration = 30.0 # Duration of the trajectory, affects speed
arm.zero_ft_sensor()
initial_pose = arm.end_effector()
trajectory = traj_utils.compute_trajectory(initial_pose, plane, radius, radius_direction,
steps, revolutions, trajectory_type="spiral", from_center=True,
wiggle_direction="X", wiggle_angle=np.deg2rad(0.0), wiggle_revolutions=1.0)
execute_trajectory(trajectory, duration=duration, use_force_control=True)
def circular_trajectory():
"""
Force/Position control. Follow a circular trajectory on the world's YZ plan while controlling force on Z
"""
initial_q = [1.57, -1.57, 1.26, -1.57, -1.57, 0]
arm.set_joint_positions(positions=initial_q, wait=True, target_time=1)
plane = "YZ"
radius = 0.02
radius_direction = "+Z"
revolutions = 1
steps = 100 # Number of waypoints of the circular trajectory
duration = 30.0 # Duration of the trajectory, affects speed
arm.zero_ft_sensor()
initial_pose = arm.end_effector()
trajectory = traj_utils.compute_trajectory(initial_pose, plane, radius, radius_direction,
steps, revolutions, trajectory_type="circular", from_center=False,
wiggle_direction="X", wiggle_angle=np.deg2rad(0.0), wiggle_revolutions=10.0)
execute_trajectory(trajectory, duration=duration, use_force_control=True)
def execute_trajectory(trajectory, duration, use_force_control=False, termination_criteria=None):
if use_force_control:
pf_model = init_force_control([1., 1., 1., 1., 1., 1.])
target_force = np.array([0., 0., 0., 0., 0., 0.])
max_force_torque = np.array([50.0, 50., 50., 5., 5., 5.])
def termination_criteria(current_pose, standby): return False # Dummy function
full_force_control(target_force, trajectory, pf_model, timeout=duration,
relative_to_ee=False, max_force_torque=max_force_torque, termination_criteria=termination_criteria)
else:
joint_trajectory = []
for point in trajectory:
joint_trajectory.append(arm._solve_ik(point))
arm.set_joint_trajectory(trajectory=joint_trajectory, target_time=duration)
def init_force_control(selection_matrix, dt=0.02):
Kp = np.array([2., 2., 2., 1., 1., 1.])
Kp_pos = Kp
Kd_pos = Kp * 0.01
Ki_pos = Kp * 0.0
position_pd = utils.PID(Kp=Kp_pos, Ki=Ki_pos, Kd=Kd_pos, dynamic_pid=True)
# Force PID gains
Kp = np.array([0.05, 0.05, 0.05, 0.05, 0.05, 0.05])
Kp_force = Kp
Kd_force = Kp * 0.0
Ki_force = Kp * 0.01
force_pd = utils.PID(Kp=Kp_force, Kd=Kd_force, Ki=Ki_force)
pf_model = ForcePositionController(
position_pd=position_pd, force_pd=force_pd, alpha=np.diag(selection_matrix), dt=dt)
return pf_model
def full_force_control(
target_force=None, target_positions=None, model=None,
selection_matrix=[1., 1., 1., 1., 1., 1.],
relative_to_ee=False, timeout=10.0, max_force_torque=[200., 200., 200., 5., 5., 5.],
termination_criteria=None):
"""
Use with caution!!
target_force: list[6], target force for each direction x,y,z,ax,ay,az
target_position: list[7], target position for each direction x,y,z + quaternion
selection_matrix: list[6], define which direction is controlled by position(1.0) or force(0.0)
relative_to_ee: bool, whether to use the base_link of the robot as frame or the ee_link (+ ee_transform)
timeout: float, duration in seconds of the force control
termination_criteria: func, optional condition that would stop the compliance controller
"""
arm.zero_ft_sensor() # offset the force sensor
arm.relative_to_ee = relative_to_ee
if model is None:
pf_model = init_force_control(selection_matrix)
else:
pf_model = model
pf_model.selection_matrix = np.diag(selection_matrix)
max_force_torque = np.array(max_force_torque)
target_force = np.array([0., 0., 0., 0., 0., 0.]
) if target_force is None else target_force
target_positions = arm.end_effector(
) if target_positions is None else np.array(target_positions)
pf_model.set_goals(force=target_force)
return arm.set_hybrid_control_trajectory(target_positions, pf_model, max_force_torque=max_force_torque,
timeout=timeout, stop_on_target_force=False,
termination_criteria=termination_criteria)
def force_control():
"""
Simple example of compliance control
selection_matrix: list[6]. define which direction is controlled by position(1.0) or force(0.0) goal.
Values in between make the controller attempt to achieve both position and force goals.
"""
arm.zero_ft_sensor()
timeout = 10.0 # Duration of the active control, does not affect speed.
selection_matrix = [1., 1., 0., 1., 1., 1.]
target_force = np.array([0., 0., 5., 0., 0., 0.])
full_force_control(
target_force, selection_matrix=selection_matrix, timeout=timeout)
def main():
""" Main function to be run. """
parser = argparse.ArgumentParser(description='Test force control')
parser.add_argument('-m', '--move', action='store_true',
help='move to joint configuration')
parser.add_argument('-f', '--force', action='store_true',
help='Force control demo')
parser.add_argument('--circle', action='store_true',
help='Circular rotation around a target pose')
parser.add_argument('--spiral', action='store_true',
help='Spiral rotation around a target pose')
parser.add_argument('--namespace', type=str,
help='Namespace of arm', default=None)
args = parser.parse_args()
rospy.init_node('ur3e_compliance_control')
ns = ''
joints_prefix = None
tcp_link = None
if args.namespace:
ns = args.namespace
joints_prefix = args.namespace + '_'
global arm
arm = CompliantController(namespace=ns,
joint_names_prefix=joints_prefix)
if args.move:
move_joints()
if args.circle:
circular_trajectory()
if args.spiral:
spiral_trajectory()
if args.force:
force_control()
if __name__ == "__main__":
main()
================================================
FILE: ur_control/scripts/controller_examples.py
================================================
#! /usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import numpy as np
import tf
from ur_control import transformations, traj_utils, conversions
from ur_control.arm import Arm
from ur_control.constants import GripperType
import argparse
import random
import rospy
import timeit
np.set_printoptions(suppress=True)
np.set_printoptions(linewidth=np.inf)
def move_joints(wait=True):
# desired joint configuration 'q'
q = [1.8391, -1.5659, 1.4889, -1.6421, -1.6115, 0.2656]
# go to desired joint configuration
# in t time (seconds)
# wait is for waiting to finish the motion before executing
# anything else or ignore and continue with whatever is next
arm.set_joint_positions(positions=q, wait=wait, target_time=0.5)
def follow_trajectory():
traj = [
[2.4463, -1.8762, -1.6757, 0.3268, 2.2378, 3.1960],
[2.5501, -1.9786, -1.5293, 0.2887, 2.1344, 3.2062],
[2.5501, -1.9262, -1.3617, 0.0687, 2.1344, 3.2062],
[2.4463, -1.8162, -1.5093, 0.1004, 2.2378, 3.1960],
[2.3168, -1.7349, -1.6096, 0.1090, 2.3669, 3.1805],
[2.3168, -1.7997, -1.7772, 0.3415, 2.3669, 3.1805],
[2.3168, -1.9113, -1.8998, 0.5756, 2.3669, 3.1805],
[2.4463, -1.9799, -1.7954, 0.5502, 2.2378, 3.1960],
[2.5501, -2.0719, -1.6474, 0.5000, 2.1344, 3.2062],
]
for t in traj:
arm.set_joint_positions(positions=t, wait=True, target_time=1.0)
def move_endeffector():
# get current position of the end effector
cpose = arm.end_effector()
# define the desired translation/rotation
deltax = np.array([0., 0., 0.04, 0., 0., 0.])
# add translation/rotation to current position
cpose = transformations.transform_pose(cpose, deltax, rotated_frame=True)
# execute desired new pose
# may fail if IK solution is not found
arm.set_target_pose(pose=cpose, wait=True, target_time=1.0)
def move_gripper():
print("closing")
arm.gripper.close()
rospy.sleep(1.0)
print("opening")
arm.gripper.open()
rospy.sleep(1.0)
print("moving")
arm.gripper.command(0.5, percentage=True) # in percentage (80%)
# 0.0 is full close, 1.0 is full open
rospy.sleep(1.0)
print("moving")
arm.gripper.command(0.01) # in meters
# 0.05 is full open, 0.0 is full close
# max gap for the Robotiq Hand-e is 0.05 meters
print("current gripper position", round(arm.gripper.get_position(), 4), "meters")
def grasp_naive():
# probably won't work
arm.gripper.open()
q1 = [1.82224, -1.59475, 1.68247, -1.80611, -1.60922, 0.24936]
arm.set_joint_positions(positions=q1, wait=True, target_time=1.0)
q2 = [1.82225, -1.55525, 1.86741, -2.03039, -1.60938, 0.24935]
arm.set_joint_positions(positions=q2, wait=True, target_time=1.0)
arm.gripper.command(0.036)
rospy.sleep(0.5)
q1 = [1.82224, -1.59475, 1.68247, -1.80611, -1.60922, 0.24936]
arm.set_joint_positions(positions=q1, wait=True, target_time=1.0)
def grasp_plugin():
arm.gripper.open()
q1 = [1.82224, -1.59475, 1.68247, -1.80611, -1.60922, 0.24936]
arm.set_joint_positions(positions=q1, wait=True, target_time=1.0)
q2 = [1.82225, -1.55525, 1.86741, -2.03039, -1.60938, 0.24935]
arm.set_joint_positions(positions=q2, wait=True, target_time=1.0)
arm.gripper.command(0.039)
# attach the object "link" to the robot "model_name"::"link_name"
arm.gripper.grab(link_name="cube3::link")
q1 = [1.82224, -1.59475, 1.68247, -1.80611, -1.60922, 0.24936]
arm.set_joint_positions(positions=q1, wait=True, target_time=1.0)
rospy.sleep(2.0) # release after 2 secs
# dettach the object "link" to the robot "model_name"::"link_name"
arm.gripper.open()
arm.gripper.release(link_name="cube3::link")
def get_random_valid_direction(plane):
if plane == "XZ":
return random.choice(["+X", "-X", "+Z", "-Z"])
elif plane == "YZ":
return random.choice(["+Y", "-Y", "+Z", "-Z"])
elif plane == "XY":
return random.choice(["+X", "-X", "+Y", "-Y"])
else:
raise ValueError("Invalid value for plane: %s" % plane)
def circular_trajectory():
""" Simple circular trajectory from initial pose. 5cm of radius"""
initial_q = [1.8391, -1.5659, 1.4889, -1.6421, -1.6115, 0.2656]
arm.set_joint_positions(positions=initial_q, wait=True, target_time=2)
duration = 5.0
steps = 100
plane = "XY"
direction = get_random_valid_direction(plane)
dummy_trajectory = traj_utils.compute_trajectory(initial_pose=[0, 0, 0., 0, 0, 0, 1.],
plane=plane, radius=0.05,
radius_direction=direction, steps=steps, revolutions=1,
from_center=False, trajectory_type="circular")
listener = tf.TransformListener()
# convert dummy_trajectory (initial pose frame id) to robot's base frame
try:
listener.waitForTransform("base_link", "wrist_3_link", rospy.Time(0), rospy.Duration(1))
transform2target = listener.fromTranslationRotation(*listener.lookupTransform("base_link", "wrist_3_link", rospy.Time(0)))
except Exception as e:
print(e)
return False
actual_trajectory = []
for p in dummy_trajectory:
ps = conversions.to_pose_stamped("base_link", p)
next_pose = conversions.from_pose_to_list(conversions.transform_pose("base_link", transform2target, ps).pose)
print("next_pose", np.round(next_pose[:3].tolist(), 4))
actual_trajectory.append(next_pose)
arm.set_target_pose(pose=next_pose, target_time=duration/steps, wait=False)
rospy.sleep(duration/steps)
arm.set_pose_trajectory(trajectory=actual_trajectory, target_time=duration)
def main():
""" Main function to be run. """
parser = argparse.ArgumentParser(description='Test force control')
parser.add_argument('-m', '--move', action='store_true',
help='move to joint configuration')
parser.add_argument('-t', '--move_traj', action='store_true',
help='move following a trajectory of joint configurations')
parser.add_argument('-e', '--move_ee', action='store_true',
help='move to a desired end-effector position')
parser.add_argument('-g', '--gripper', action='store_true',
help='Move gripper')
parser.add_argument('--grasp_naive', action='store_true',
help='Test simple grasping (cube_tasks world)')
parser.add_argument('--grasp_plugin', action='store_true',
help='Test grasping plugin (cube_tasks world)')
parser.add_argument('--circle', action='store_true',
help='Circular rotation around a target pose')
args = parser.parse_args()
rospy.init_node('ur3e_script_control')
global arm
arm = Arm(gripper_type=GripperType.GENERIC)
real_start_time = timeit.default_timer()
ros_start_time = rospy.get_time()
if args.move:
move_joints()
if args.move_traj:
follow_trajectory()
if args.move_ee:
move_endeffector()
if args.gripper:
move_gripper()
if args.grasp_naive:
grasp_naive()
if args.grasp_plugin:
grasp_plugin()
if args.circle:
circular_trajectory()
print("real time", round(timeit.default_timer() - real_start_time, 3))
print("ros time", round(rospy.get_time() - ros_start_time, 3))
if __name__ == "__main__":
main()
================================================
FILE: ur_control/scripts/ft_filter.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2023 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import argparse
import collections
import numpy as np
import rospy
from std_srvs.srv import Empty, EmptyResponse, SetBool, SetBoolResponse
from geometry_msgs.msg import WrenchStamped
from ur_control import spalg, utils, filters, conversions
class FTsensor(object):
def __init__(self, in_topic, namespace="", out_topic=None,
sampling_frequency=500, cutoff=10,
order=2, data_window=10,
republish=False):
self.ns = namespace
self.enable_publish = republish
self.enable_filtering = True
self.in_topic = utils.solve_namespace(namespace + "/" + in_topic)
if out_topic:
self.out_topic = utils.solve_namespace(namespace + "/" + out_topic)
self.out_tcp_topic = utils.solve_namespace(namespace + "/" + out_topic) + "tcp"
else:
self.out_topic = utils.solve_namespace(self.in_topic + 'filtered')
self.out_tcp_topic = self.in_topic + "tcp"
rospy.loginfo("Publishing filtered FT to %s" % self.out_topic)
# Load previous offset to zero filtered signal
self.wrench_offset = rospy.get_param('%sft_offset' % self.out_topic, None)
self.wrench_offset = np.zeros(6) if self.wrench_offset is None else self.wrench_offset
# Publisher to outward topic
self.pub = rospy.Publisher(self.out_topic, WrenchStamped, queue_size=1)
# Publish a wrench transformed/converted to a TCP point
self.pub_tcp = rospy.Publisher(self.out_tcp_topic, WrenchStamped, queue_size=1)
# Service for zeroing the filtered signal
rospy.Service(self.out_topic + "zero_ftsensor", Empty, self._srv_zeroing)
rospy.Service(self.out_topic + "enable_publish", SetBool, self._srv_publish)
rospy.Service(self.out_topic + "enable_filtering", SetBool, self._srv_filtering)
# Low pass filter
self.filter = filters.ButterLowPass(cutoff, sampling_frequency, order)
self.data_window = data_window
assert (self.data_window >= 5)
self.data_queue = collections.deque(maxlen=self.data_window)
# Subscribe to incoming topic
rospy.Subscriber(self.in_topic, WrenchStamped, self.raw_wrench_cb, queue_size=1)
rospy.loginfo('FT filter successfully initialized')
rospy.sleep(1) # wait some time to fill the filter
def add_wrench_observation(self, wrench):
self.data_queue.append(np.array(wrench))
def raw_wrench_cb(self, msg):
if rospy.is_shutdown():
return
self._active = True
current_wrench = conversions.from_wrench(msg.wrench)
self.add_wrench_observation(current_wrench)
if self.enable_publish:
if self.enable_filtering:
current_wrench = self.get_filtered_wrench()
if current_wrench is not None:
data = current_wrench - self.wrench_offset
msg = WrenchStamped()
msg.wrench = conversions.to_wrench(data)
self.pub.publish(msg)
if rospy.has_param(self.out_tcp_topic+"/pose_sensor_to_tcp"):
# Convert torques to force at a TCP point
pose_sensor_to_tcp = rospy.get_param(self.out_tcp_topic+"/pose_sensor_to_tcp")
tcp_wrench = data.copy()
tcp_wrench[:3] += spalg.sensor_torque_to_tcp_force(tcp_position=pose_sensor_to_tcp, sensor_torques=current_wrench[3:])
tcp_wrench[3:] = np.zeros(3)
msg = WrenchStamped()
msg.wrench = conversions.to_wrench(tcp_wrench)
self.pub_tcp.publish(msg)
# function to filter out high frequency signal
def get_filtered_wrench(self):
if len(self.data_queue) < self.data_window:
return None
wrench_filtered = self.filter(np.array(self.data_queue))
return wrench_filtered[-1, :]
def update_wrench_offset(self):
current_wrench = self.get_filtered_wrench()
if current_wrench is not None:
self.wrench_offset = current_wrench
if self.wrench_offset is not None:
rospy.set_param('%sft_offset' % self.out_topic, self.wrench_offset.tolist())
def set_enable_publish(self, enable):
self.enable_publish = enable
def set_enable_filtering(self, enable):
self.enable_filtering = enable
def _srv_zeroing(self, req):
self.update_wrench_offset()
return EmptyResponse()
def _srv_publish(self, req):
self.set_enable_publish(req.data)
return SetBoolResponse(success=True)
def _srv_filtering(self, req):
self.set_enable_filtering(req.data)
return SetBoolResponse(success=True)
def main():
""" Main function to be run. """
parser = argparse.ArgumentParser(description='Filter FT signal')
parser.add_argument('-ns', '--namespace', type=str, help='Namespace', required=False, default="")
parser.add_argument('-t', '--ft_topic', type=str, help='FT sensor data topic', required=True)
parser.add_argument('-ot', '--out_topic', type=str, help='Topic where filtered data will be published')
parser.add_argument('-z', '--zero', action='store_true', help='Zero FT signal')
args, _ = parser.parse_known_args()
rospy.init_node('ft_filter')
out_topic = None if not args.out_topic else args.out_topic
ft_sensor = FTsensor(namespace=args.namespace, in_topic=args.ft_topic, out_topic=out_topic, republish=True)
if args.zero:
ft_sensor.update_wrench_offset()
rospy.spin()
main()
================================================
FILE: ur_control/scripts/getch.py
================================================
import sys
import termios
import tty
from select import select
def getch(timeout=0.01):
"""
Retrieves a character from stdin.
Returns None if no character is available within the timeout.
Blocks if timeout < 0.
"""
# If this is being piped to, ignore non-blocking functionality
if not sys.stdin.isatty():
return sys.stdin.read(1)
fileno = sys.stdin.fileno()
old_settings = termios.tcgetattr(fileno)
ch = None
try:
tty.setraw(fileno)
rlist = [fileno]
if timeout >= 0:
[rlist, _, _] = select(rlist, [], [], timeout)
if fileno in rlist:
ch = sys.stdin.read(1)
except Exception as ex:
print(("getch", ex))
raise OSError
finally:
termios.tcsetattr(fileno, termios.TCSADRAIN, old_settings)
return ch
================================================
FILE: ur_control/scripts/imu.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import argparse
import rospy
import numpy as np
from ur_control import conversions, utils
from sensor_msgs.msg import Imu
class ImuFake(object):
def __init__(self, topic, namespace="", frequency=500):
self.ns = namespace
self.topic = utils.solve_namespace(namespace + "/" + topic)
# Publisher to outward topic
self.pub = rospy.Publisher(self.topic, Imu, queue_size=10)
prefix = "" if not namespace else namespace + "_"
base_link = "base_link"
gravity_on_base_link = np.array([0, 0, 9.81])
rate = rospy.Rate(frequency)
while not rospy.is_shutdown():
msg = Imu()
msg.header.frame_id = prefix + base_link
msg.header.stamp = rospy.Time.now()
msg.linear_acceleration = conversions.to_vector3(gravity_on_base_link)
self.pub.publish(msg)
try:
rate.sleep()
except rospy.ROSInterruptException:
pass
def main():
""" Main function to be run. """
parser = argparse.ArgumentParser(description='Filter FT signal')
parser.add_argument('-ns', '--namespace', type=str, help='Namespace', required=False, default="")
args, unknown = parser.parse_known_args()
rospy.init_node('imu_fake')
ft_sensor = ImuFake(topic="imu", namespace=args.namespace)
rospy.spin()
main()
================================================
FILE: ur_control/scripts/joint_position_keyboard.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
"""
UR Joint Position Example: keyboard
"""
import argparse
import rospy
from ur_control.arm import Arm
from ur_control import transformations
import getch
import numpy as np
from ur_control.constants import GripperType
np.set_printoptions(linewidth=np.inf)
np.set_printoptions(suppress=True)
def map_keyboard():
def print_robot_state():
print("Joint angles:", np.round(arm.joint_angles(), 4).tolist())
print("EE Pose:", np.round(arm.end_effector(), 5).tolist())
print("EE Pose (euler):", np.round(arm.end_effector(rot_type="euler"), 5).tolist())
if arm.gripper:
print("Gripper angle:", np.round(arm.gripper.get_position(), 4))
print("Gripper position:", np.round(arm.gripper.opening_width, 4))
print("Gripper percentage:", np.round(arm.gripper.get_opening_percentage(), 4))
def set_j(joint_name, sign):
global delta_q
current_position = arm.joint_angles()
current_position[joint_name] += delta_q * sign
arm.set_joint_positions(positions=current_position, target_time=0.25)
def update_d(delta, increment):
if delta == 'q':
global delta_q
delta_q += np.deg2rad(increment)
print(("delta_q", np.rad2deg(delta_q)))
if delta == 'x':
global delta_x
delta_x += increment
print(("delta_x", delta_x))
def set_pose_ik(dim, sign):
global delta_x
global delta_q
x = arm.end_effector()
delta = np.zeros(6)
if dim <= 2: # position
delta[dim] += delta_x * sign
else: # rotation
delta[dim] += delta_q * sign
xc = transformations.transform_pose(x, delta, rotated_frame=relative_to_tcp)
arm.set_target_pose(pose=xc, target_time=0.25)
def open_gripper():
arm.gripper.open()
def close_gripper():
arm.gripper.close()
def move_gripper(delta):
cpose = arm.gripper.get_position()
cpose += delta
arm.gripper.command(cpose)
global delta_q
global delta_x
delta_q = np.deg2rad(1.0)
delta_x = 0.005
bindings = {
# key: (function, args, description)
'z': (set_j, [0, 1], "shoulder_pan_joint increase"),
'v': (set_j, [0, -1], "shoulder_pan_joint decrease"),
'x': (set_j, [1, 1], "shoulder_lift_joint increase"),
'c': (set_j, [1, -1], "shoulder_lift_joint decrease"),
'a': (set_j, [2, 1], "elbow_joint increase"),
'f': (set_j, [2, -1], "elbow_joint decrease"),
's': (set_j, [3, 1], "wrist_1_joint increase"),
'd': (set_j, [3, -1], "wrist_1_joint decrease"),
'q': (set_j, [4, 1], "wrist_2_joint increase"),
'r': (set_j, [4, -1], "wrist_2_joint decrease"),
'w': (set_j, [5, 1], "wrist_3_joint increase"),
'e': (set_j, [5, -1], "wrist_3_joint decrease"),
'p': (print_robot_state, [], "right: printing"),
# Task Space
'h': (set_pose_ik, [0, 1], "x increase"),
'k': (set_pose_ik, [0, -1], "x decrease"),
'y': (set_pose_ik, [1, 1], "y increase"),
'i': (set_pose_ik, [1, -1], "y decrease"),
'u': (set_pose_ik, [2, 1], "z increase"),
'j': (set_pose_ik, [2, -1], "z decrease"),
'n': (set_pose_ik, [3, 1], "ax increase"),
'm': (set_pose_ik, [3, -1], "ax decrease"),
',': (set_pose_ik, [4, 1], "ay increase"),
'.': (set_pose_ik, [4, -1], "ay decrease"),
'o': (set_pose_ik, [5, 1], "az increase"),
'l': (set_pose_ik, [5, -1], "az decrease"),
# Increase or decrease delta
'1': (update_d, ['q', 0.25], "delta_q increase"),
'2': (update_d, ['q', -0.25], "delta_q decrease"),
'6': (update_d, ['x', 0.0001], "delta_x increase"),
'7': (update_d, ['x', -0.0001], "delta_x decrease"),
# Gripper
'5': (move_gripper, [0.005], "open gripper a bit"),
't': (open_gripper, [], "open gripper"),
'g': (close_gripper, [], "close gripper"),
'b': (move_gripper, [-0.005], "close gripper a bit"),
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = getch.getch()
if c:
# catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
# expand binding to something like "set_j(right, 's0', 0.1)"
cmd[0](*cmd[1])
print(("command: %s" % (cmd[2], )))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(
list(bindings.items()), key=lambda x: x[1][2]):
print((" %s: %s" % (key, val[2])))
def main():
"""Joint Position Example: Keyboard Control
Use your dev machine's keyboard to control joint positions.
Each key corresponds to increasing or decreasing the angle
of a joint on one of Baxter's arms. Each arm is represented
by one side of the keyboard and inner/outer key pairings
on each row for each joint.
"""
epilog = """
See help inside the example with the '?' key for key bindings.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(
formatter_class=arg_fmt, description=main.__doc__, epilog=epilog)
parser.add_argument(
'--relative', action='store_true', help='Motion Relative to ee')
parser.add_argument(
'--namespace', type=str, help='Namespace of arm (useful when having multiple arms)', default=None)
parser.add_argument(
'--gripper', type=str, help='Load gripper controller, indicate the gripper type (ROBOTIQ or GENERIC)', default="")
parser.add_argument(
'--tcp', type=str, help='Tool Center Point or End-Effector frame for IK without joint prefix', default='tool0')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node("joint_position_keyboard", log_level=rospy.INFO)
global relative_to_tcp
relative_to_tcp = args.relative
tcp_link = args.tcp
joints_prefix = args.namespace + '_' if args.namespace else None
if args.gripper:
if str(args.gripper).lower() == "robotiq":
gripper = GripperType.ROBOTIQ
elif str(args.gripper).lower() == "generic":
gripper = GripperType.GENERIC
else:
raise ValueError(f"Invalid gripper type `{args.gripper}`. Supported types: ROBOTIQ or GENERIC")
else:
gripper = None
global arm
arm = Arm(namespace=args.namespace,
gripper_type=gripper,
joint_names_prefix=joints_prefix,
ee_link=tcp_link)
map_keyboard()
print("Done.")
if __name__ == '__main__':
main()
================================================
FILE: ur_control/scripts/joint_position_mouse6d.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
"""
UR Joint Position Example: 3Dconnexion mouse
requires ros-$ROS-VERSION-spacenav-node
and to launch roslaunch spacenav_node classic.launch
#TODO launch automatically
"""
import argparse
import rospy
from ur_control.arm import Arm
from ur_control.mouse_6d import Mouse6D
from ur_control import transformations
from ur_ikfast import ur_kinematics
import numpy as np
np.set_printoptions(suppress=True)
ur3e_arm = ur_kinematics.URKinematics('ur3e')
mouse6d = Mouse6D()
axes = 'rxyz'
def e2q(e):
return transformations.quaternion_from_euler(e[0], e[1], e[2], axes=axes)
def print_robot_state():
print(("Joint angles:", np.round(arm.joint_angles(), 3)))
print(("End Effector:", np.round(arm.end_effector(rot_type='euler'), 3)))
def start_control(motion_type="linear"):
print("Start moving. type", motion_type)
rate = rospy.Rate(125)
delta_x = 0.01
delta_q = np.deg2rad(1)
while not rospy.is_shutdown():
x = arm.end_effector()
xd = np.array(mouse6d.twist)
xd[:3] = [delta_x*np.sign(xd[i]) if abs(xd[i]) > 0.15 else 0.0 for i in range(3)]
xd[3:] = [delta_q*np.sign(xd[3+i]) if abs(xd[3+i]) > 0.15 else 0.0 for i in range(3)]
if motion_type == "rotated":
xd[2] *= -1
elif motion_type == "linear":
pass
else:
print("motion_type not supported", motion_type)
break
x = transformations.pose_from_angular_velocity(x, xd, dt=0.25)
if mouse6d.joy_buttons[0] == 1:
print_robot_state()
arm.set_target_pose_flex(pose=x, t=0.25)
rate.sleep()
def main():
""" 3D mouse Control """
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(
formatter_class=arg_fmt, description=main.__doc__)
parser.add_argument('-r', action='store_true', help='move using relative rotation of end-effector')
parser.add_argument(
'--robot', action='store_true', help='for the real robot')
parser.add_argument(
'--beta', action='store_true', help='for the real robot. beta driver')
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node("joint_position_keyboard")
global arm
arm = Arm(ft_sensor=False)
start_control()
print("Done.")
if __name__ == '__main__':
main()
================================================
FILE: ur_control/scripts/moveit_tutorial.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import argparse
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import signal
def signal_handler(sig, frame):
print('You pressed Ctrl+C!')
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
def all_close(goal, actual, tolerance):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@returns: bool
"""
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
class MoveGroupPythonIntefaceTutorial(object):
"""MoveGroupPythonIntefaceTutorial"""
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
# BEGIN_SUB_TUTORIAL setup
##
# First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
# Instantiate a `RobotCommander`_ object. Provides information such as the robot's
# kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
# Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
# for getting, setting, and updating the robot's internal understanding of the
# surrounding world:
scene = moveit_commander.PlanningSceneInterface()
# Instantiate a `MoveGroupCommander`_ object. This object is an interface
# to a planning group (group of joints). In this tutorial the group is the primary
# arm joints in the ur3e robot, so we set the group's name to "ur3e_arm".
# If you are using a different robot, change this value to the name of your robot
# arm planning group.
# This interface can be used to plan and execute motions:
group_name = "arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
# Create a `DisplayTrajectory`_ ROS publisher which is used to display
# trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = move_group.get_planning_frame()
self.eef_link = move_group.get_end_effector_link()
self.group_names = robot.get_group_names()
def display_basic_info(self):
# BEGIN_SUB_TUTORIAL basic_info
##
# Getting Basic Information
# ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
print("============ Planning frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
print("============ End effector link: %s" % self.eef_link)
# We can get a list of all the groups in the robot:
print("============ Available Planning Groups:", self.group_names)
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(self.robot.get_current_state())
print("")
# END_SUB_TUTORIAL
def go_to_joint_state(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
move_group = self.move_group
# BEGIN_SUB_TUTORIAL plan_to_joint_state
##
# Planning to a Joint Goal
# ^^^^^^^^^^^^^^^^^^^^^^^^
# The ur3e's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
# thing we want to do is move it to a slightly better configuration.
# We can get the joint values from the group and adjust some of the values:
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 1.57
joint_goal[1] = -1.57
joint_goal[2] = 1.30
joint_goal[3] = -1.57
joint_goal[4] = -1.57
joint_goal[5] = 0
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
move_group.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
# END_SUB_TUTORIAL
# For testing:
print("Current pose", move_group.get_current_pose().pose)
current_joints = move_group.get_current_joint_values()
return all_close(joint_goal, current_joints, 0.01)
def go_to_pose_goal(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
move_group = self.move_group
# BEGIN_SUB_TUTORIAL plan_to_pose
##
# Planning to a Pose Goal
# ^^^^^^^^^^^^^^^^^^^^^^^
# We can plan a motion for this group to a desired pose for the
# end-effector:
pose_goal = move_group.get_current_pose().pose
pose_goal.position.y += -0.1
move_group.set_pose_target(pose_goal)
# Now, we call the planner to compute the plan and execute it.
plan = move_group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
move_group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
move_group.clear_pose_targets()
# END_SUB_TUTORIAL
# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_pose = self.move_group.get_current_pose().pose
return all_close(pose_goal, current_pose, 0.01)
def plan_cartesian_path(self, scale=1):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
move_group = self.move_group
# BEGIN_SUB_TUTORIAL plan_cartesian_path
##
# Cartesian Paths
# ^^^^^^^^^^^^^^^
# You can plan a Cartesian path directly by specifying a list of waypoints
# for the end-effector to go through. If executing interactively in a
# Python shell, set scale = 1.0.
##
waypoints = []
wpose = move_group.get_current_pose().pose
wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scale * 0.2 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
wpose.position.x += scale * 0.1 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
wpose.position.y -= scale * 0.1 # Third move sideways (y)
waypoints.append(copy.deepcopy(wpose))
# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation. We will disable the jump threshold by setting it to 0.0,
# ignoring the check for infeasible jumps in joint space, which is sufficient
# for this tutorial.
(plan, fraction) = move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction
# END_SUB_TUTORIAL
def display_trajectory(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
robot = self.robot
display_trajectory_publisher = self.display_trajectory_publisher
# BEGIN_SUB_TUTORIAL display_trajectory
##
# Displaying a Trajectory
# ^^^^^^^^^^^^^^^^^^^^^^^
# You can ask RViz to visualize a plan (aka trajectory) for you. But the
# group.plan() method does this automatically so this is not that useful
# here (it just displays the same trajectory again):
##
# A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory.
# We populate the trajectory_start with our current robot state to copy over
# any AttachedCollisionObjects and add our plan to the trajectory.
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
# END_SUB_TUTORIAL
def execute_plan(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
move_group = self.move_group
# BEGIN_SUB_TUTORIAL execute_plan
##
# Executing a Plan
# ^^^^^^^^^^^^^^^^
# Use execute if you would like the robot to follow
# the plan that has already been computed:
move_group.execute(plan, wait=True)
# **Note:** The robot's current joint state must be within some tolerance of the
# first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail
# END_SUB_TUTORIAL
def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
# BEGIN_SUB_TUTORIAL wait_for_scene_update
##
# Ensuring Collision Updates Are Receieved
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# If the Python node dies before publishing a collision object update message, the message
# could get lost and the box will not appear. To ensure that the updates are
# made, we wait until we see the changes reflected in the
# ``get_attached_objects()`` and ``get_known_object_names()`` lists.
# For the purpose of this tutorial, we call this function after adding,
# removing, attaching or detaching an object in the planning scene. We then wait
# until the updates have been made or ``timeout`` seconds have passed
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# Test if the box is in attached objects
attached_objects = scene.get_attached_objects([box_name])
is_attached = len(attached_objects.keys()) > 0
# Test if the box is in the scene.
# Note that attaching the box will remove it from known_objects
is_known = box_name in scene.get_known_object_names()
# Test if we are in the expected state
if (box_is_attached == is_attached) and (box_is_known == is_known):
return True
# Sleep so that we give other threads time on the processor
rospy.sleep(0.1)
seconds = rospy.get_time()
# If we exited the while loop without returning then we timed out
return False
# END_SUB_TUTORIAL
def add_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
# BEGIN_SUB_TUTORIAL add_box
##
# Adding Objects to the Planning Scene
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# First, we will create a box in the planning scene at the location of the left finger:
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "gripper_tip_link"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.z = 0.0 # slightly above the end effector
box_name = "box_name"
scene.add_box(box_name, box_pose, size=(0.04, 0.04, 0.04))
# END_SUB_TUTORIAL
# Copy local variables back to class variables. In practice, you should use the class
# variables directly unless you have a good reason not to.
self.box_name = box_name
return self.wait_for_state_update(box_is_known=True, timeout=timeout)
def add_box2(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
# BEGIN_SUB_TUTORIAL add_box
##
# Adding Objects to the Planning Scene
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# First, we will create a box in the planning scene at the location of the left finger:
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "base_link"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = 0.93 # slightly above the end effector
box_pose.pose.position.y = 0.25 # slightly above the end effector
box_pose.pose.position.z = 1.02 # slightly above the end effector
box_name = "box_name"
scene.add_box(box_name, box_pose, size=(0.03, 0.03, 0.03))
# END_SUB_TUTORIAL
# Copy local variables back to class variables. In practice, you should use the class
# variables directly unless you have a good reason not to.
self.box_name = box_name
return self.wait_for_state_update(box_is_known=True, timeout=timeout)
def attach_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
robot = self.robot
scene = self.scene
eef_link = self.eef_link
group_names = self.group_names
# BEGIN_SUB_TUTORIAL attach_object
##
# Attaching Objects to the Robot
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# Next, we will attach the box to the ur3e wrist. Manipulating objects requires the
# robot be able to touch them without the planning scene reporting the contact as a
# collision. By adding link names to the ``touch_links`` array, we are telling the
# planning scene to ignore collisions between those links and the box. For the ur3e
# robot, we set ``grasping_group = 'hand'``. If you are using a different robot,
# you should change this value to the name of your end effector group name.
grasping_group = 'gripper'
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)
# END_SUB_TUTORIAL
# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout)
def detach_box(self, name=None, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name if name is None else name
scene = self.scene
eef_link = self.eef_link
# BEGIN_SUB_TUTORIAL detach_object
##
# Detaching Objects from the Robot
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# We can also detach and remove the object from the planning scene:
scene.remove_attached_object(eef_link, name=box_name)
# END_SUB_TUTORIAL
# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout)
def remove_box(self, name=None, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name if name is None else name
scene = self.scene
# BEGIN_SUB_TUTORIAL remove_object
##
# Removing Objects from the Planning Scene
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# We can remove the box from the world.
scene.remove_world_object(box_name)
# **Note:** The object must be detached before we can remove it from the world
# END_SUB_TUTORIAL
# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)
def main():
"""Moveit Control Example
Test several functionalities of MoveIt with the Python interface.
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(
formatter_class=arg_fmt, description=main.__doc__)
parser.add_argument(
'-j', '--move-joints', action='store_true', help='Movement using joint state goal')
parser.add_argument(
'-p', '--move-pose', action='store_true', help='Movement using a pose goal')
parser.add_argument(
'-a', '--add-box', action='store_true', help='Add & Attach a box to the gripper')
parser.add_argument(
'-r', '--remove-box', action='store_true', help='Detach & Remove box')
parser.add_argument(
'-d', '--detach-box', action='store_true', help='Detach box')
parser.add_argument(
'--tutorial', action='store_true', help='Execute original tutorial sequence')
args = parser.parse_args(rospy.myargv()[1:])
tutorial = MoveGroupPythonIntefaceTutorial()
rospy.sleep(1)
if args.move_joints:
tutorial.display_basic_info()
tutorial.go_to_joint_state()
elif args.move_pose:
tutorial.display_basic_info()
tutorial.go_to_pose_goal()
elif args.add_box:
tutorial.add_box2()
elif args.remove_box:
tutorial.remove_box()
elif args.detach_box:
tutorial.detach_box()
elif args.tutorial:
try:
print("")
print("----------------------------------------------------------")
print("Welcome to the MoveIt MoveGroup Python Interface Tutorial")
print("----------------------------------------------------------")
print("Press Ctrl-D to exit at any time")
print("")
print("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...")
input()
tutorial.display_basic_info()
print("============ Press `Enter` to execute a movement using a joint state goal ...")
input()
tutorial.go_to_joint_state()
print("============ Press `Enter` to execute a movement using a pose goal ...")
input()
tutorial.go_to_pose_goal()
print("============ Press `Enter` to plan and display a Cartesian path ...")
input()
cartesian_plan, fraction = tutorial.plan_cartesian_path()
print("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...")
input()
tutorial.display_trajectory(cartesian_plan)
print("============ Press `Enter` to execute a saved path ...")
input()
tutorial.execute_plan(cartesian_plan)
print("============ Press `Enter` to add a box to the planning scene ...")
input()
tutorial.add_box()
print("============ Press `Enter` to attach a Box to the ur3e robot ...")
input()
tutorial.attach_box()
print("============ Press `Enter` to plan and execute a path with an attached collision object ...")
input()
cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)
tutorial.execute_plan(cartesian_plan)
print("============ Press `Enter` to detach the box from the ur3e robot ...")
input()
tutorial.detach_box()
print("============ Press `Enter` to remove the box from the planning scene ...")
input()
tutorial.remove_box()
print("============ Python tutorial demo complete!")
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ == '__main__':
main()
# BEGIN_TUTORIAL
# .. _moveit_commander:
# http://docs.ros.org/melodic/api/moveit_commander/html/namespacemoveit__commander.html
##
# .. _MoveGroupCommander:
# http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html
##
# .. _RobotCommander:
# http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1robot_1_1RobotCommander.html
##
# .. _PlanningSceneInterface:
# http://docs.ros.org/melodic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html
##
# .. _DisplayTrajectory:
# http://docs.ros.org/melodic/api/moveit_msgs/html/msg/DisplayTrajectory.html
##
# .. _RobotTrajectory:
# http://docs.ros.org/melodic/api/moveit_msgs/html/msg/RobotTrajectory.html
##
# .. _rospy:
# http://docs.ros.org/melodic/api/rospy/html/
# CALL_SUB_TUTORIAL imports
# CALL_SUB_TUTORIAL setup
# CALL_SUB_TUTORIAL basic_info
# CALL_SUB_TUTORIAL plan_to_joint_state
# CALL_SUB_TUTORIAL plan_to_pose
# CALL_SUB_TUTORIAL plan_cartesian_path
# CALL_SUB_TUTORIAL display_trajectory
# CALL_SUB_TUTORIAL execute_plan
# CALL_SUB_TUTORIAL add_box
# CALL_SUB_TUTORIAL wait_for_scene_update
# CALL_SUB_TUTORIAL attach_object
# CALL_SUB_TUTORIAL detach_object
# CALL_SUB_TUTORIAL remove_object
# END_TUTORIAL
================================================
FILE: ur_control/scripts/wrench_republish.py
================================================
#!/usr/bin/env python
# The MIT License (MIT)
#
# Copyright (c) 2018-2021 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import argparse
import rospy
import numpy as np
from ur_control import utils, arm, conversions
from geometry_msgs.msg import WrenchStamped
class FTsensor(object):
def __init__(self):
robot_name = "b_bot"
tcp_link = "knife_center"
self.arm = arm.Arm(ft_topic="wrench", namespace=robot_name,
joint_names_prefix=robot_name+'_', robot_urdf=robot_name,
robot_urdf_package='trufus_scene_description',
ee_link=tcp_link)
self.in_topic = '/wrench'
self.out_topic = '/test/wrench'
rospy.loginfo("Publishing filtered FT to %s" % self.out_topic)
# Publisher to outward topic
self.pub = rospy.Publisher(self.out_topic, WrenchStamped, queue_size=10)
# Subscribe to incoming topic
rospy.Subscriber(self.in_topic, WrenchStamped, self.cb_raw)
rospy.loginfo('Pub successfully initialized')
def cb_raw(self, msg):
if rospy.is_shutdown():
return
msg_out = WrenchStamped()
msg_out.wrench = conversions.to_wrench(self.arm.get_ee_wrench(base_frame_control=True))
self.pub.publish(msg_out)
def main():
""" Main function to be run. """
rospy.init_node('ft_filter')
FTsensor()
rospy.spin()
main()
================================================
FILE: ur_control/setup.py
================================================
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['ur_control'],
package_dir={'': 'src'}
)
setup(**d)
================================================
FILE: ur_control/src/ur_control/__init__.py
================================================
================================================
FILE: ur_control/src/ur_control/arm.py
================================================
# The MIT License (MIT)
#
# Copyright (c) 2018-2023 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import collections
import numpy as np
import rospy
from geometry_msgs.msg import WrenchStamped
from std_srvs.srv import Empty, SetBool, Trigger
from ur_control import utils, spalg, conversions, transformations
from ur_control.exceptions import InverseKinematicsException
from ur_control.controllers_connection import ControllersConnection
from ur_control.controllers import JointTrajectoryController
from ur_control.grippers import GripperController, RobotiqGripper
from ur_control.constants import BASE_LINK, EE_LINK, JOINT_TRAJECTORY_CONTROLLER, FT_SUBSCRIBER, \
ExecutionResult, IKSolverType, GripperType, \
get_arm_joint_names
from ur_control.ur_services import URServices
try:
from ur_ikfast import ur_kinematics as ur_ikfast
except ImportError:
print("Import ur_ikfast not available, IKFAST would not be supported without it")
from ur_pykdl import ur_kinematics
from trac_ik_python.trac_ik import IK as TRACK_IK_SOLVER
cprint = utils.TextColors()
class Arm(object):
""" Universal Robots arm controller """
def __init__(self,
namespace: str = None,
ik_solver: IKSolverType = IKSolverType.TRAC_IK,
gripper_type: GripperType = GripperType.GENERIC,
ft_topic: str = None,
base_link: str = None,
ee_link: str = None,
joint_names_prefix: str = None):
"""
Parameters
----------
namespace : optional
ROS namespace of the robot e.g., '/ns/robot_description'
ik_solver : optional
inverse kinematic solver to be used
gripper_type : optional
gripper control approach. Generic or specific for real Robotiq gripper
ft_topic: optional
topic from which to read the wrench
base_link: optional
robot base frame. Excluding the prefix used in 'joint_names_prefix'
ee_link: optional
end-effector frame. Excluding the prefix used in 'joint_names_prefix'
joint_names_prefix: optional
optionally specify a prefix when multiple robots are defined. For example,
if 'a_bot' is defined, all joints will be consider like 'a_bot_link_name'
Raises
------
ValueError
If a non-supported gripper type or ik solver is defined
"""
self.ns = utils.solve_namespace(namespace)
base_link = utils.resolve_parameter(value=base_link, default_value=BASE_LINK)
ee_link = utils.resolve_parameter(value=ee_link, default_value=EE_LINK)
# Support for joint prefixes
self.joint_names_prefix = utils.resolve_parameter(joint_names_prefix, '')
self.base_link = base_link if joint_names_prefix is None else joint_names_prefix + base_link
self.ee_link = ee_link if joint_names_prefix is None else joint_names_prefix + ee_link
self.ik_solver = ik_solver
self.ft_topic = utils.resolve_parameter(value=ft_topic, default_value=FT_SUBSCRIBER)
self.current_ft_value = np.zeros(6)
self.wrench_queue = collections.deque(maxlen=25) # store history of FT data
# self.max_joint_speed = np.deg2rad([100, 100, 100, 200, 200, 200]) # deg/s -> rad/s
self.max_joint_speed = np.deg2rad([191, 191, 191, 371, 371, 371])
cprint.ok("Initializing ur robot with parameters")
cprint.ok("gripper: {}, ft_sensor_topic: {}, \nbase_link: {}, ee_link: {}"
.format(gripper_type, self.ft_topic, self.base_link, self.ee_link))
self.__init_controllers__(gripper_type, joint_names_prefix)
self.__init_ik_solver__(self.base_link, self.ee_link)
self.__init_ft_sensor__()
self.controller_manager = ControllersConnection(self.ns)
self.dashboard_services = URServices(self.ns)
### private methods ###
def __init_controllers__(self, gripper_type, joint_names_prefix=None):
self.joint_names = None if joint_names_prefix is None else get_arm_joint_names(joint_names_prefix)
self.joint_traj_controller = JointTrajectoryController(publisher_name=JOINT_TRAJECTORY_CONTROLLER,
namespace=self.ns,
joint_names=self.joint_names,
timeout=1.0)
self.gripper = None
if not gripper_type:
rospy.logwarn("Loading without gripper")
return
if gripper_type == GripperType.GENERIC:
self.gripper = GripperController(namespace=self.ns, prefix=self.joint_names_prefix, timeout=2.0)
elif gripper_type == GripperType.ROBOTIQ:
self.gripper = RobotiqGripper(namespace=self.ns, prefix=self.joint_names_prefix, timeout=2.0)
else:
raise ValueError("Invalid gripper type %s" % gripper_type)
def __init_ik_solver__(self, base_link, ee_link):
# Instantiate KDL kinematics solver to compute forward kinematics
if rospy.has_param("robot_description"):
self.kdl = ur_kinematics(base_link=base_link, ee_link=ee_link)
else:
raise ValueError("robot_description not found in the parameter server")
# Instantiate Inverse kinematics solver
if self.ik_solver == IKSolverType.IKFAST:
# IKfast libraries
try:
# TODO use the parameter robot_description
self.arm_ikfast = ur_ikfast.URKinematics(self._robot_urdf)
except Exception:
raise ValueError("IK solver set to IKFAST but no ikfast found for: %s. " % self._robot_urdf)
elif self.ik_solver == IKSolverType.TRAC_IK:
try:
self.trac_ik = TRACK_IK_SOLVER(base_link=base_link, tip_link=ee_link, solve_type="Distance")
except Exception as e:
rospy.logerr("Could not instantiate TRAC_IK" + str(e))
elif self.ik_solver == IKSolverType.KDL:
pass
else:
raise Exception("unsupported ik_solver", self.ik_solver)
def __init_ft_sensor__(self):
# Publisher of wrench
ft_namespace = self.ns + self.ft_topic + '/filtered'
if not utils.topic_exist(ft_namespace):
rospy.logwarn("Filtered FT topic not found. Using raw sensor directly.")
# Try the raw FT topic
ft_namespace = self.ns + self.ft_topic
rospy.Subscriber(ft_namespace, WrenchStamped, self.__ft_callback__)
self._zero_ft_filtered = lambda: None
self._ft_filtered = lambda: None
else:
rospy.Subscriber(ft_namespace, WrenchStamped, self.__ft_callback__)
self._zero_ft_filtered = rospy.ServiceProxy('%s/%s/filtered/zero_ftsensor' % (self.ns, self.ft_topic), Empty)
self._zero_ft_filtered.wait_for_service(rospy.Duration(2.0))
if not rospy.has_param("use_gazebo_sim"):
self._zero_ft = rospy.ServiceProxy('%s/ur_hardware_interface/zero_ftsensor' % self.ns, Trigger)
self._zero_ft.wait_for_service(rospy.Duration(2.0))
self._ft_filtered = rospy.ServiceProxy('%s/%s/filtered/enable_filtering' % (self.ns, self.ft_topic), SetBool)
self._ft_filtered.wait_for_service(rospy.Duration(1.0))
# Check that the FT topic is publishing
if not utils.wait_for(lambda: self.current_ft_value is not None, timeout=2.0):
rospy.logerr('Timed out waiting for {0} topic'.format(ft_namespace))
def __ft_callback__(self, msg):
self.current_ft_value = conversions.from_wrench(msg.wrench)
self.wrench_queue.append(self.current_ft_value)
### Data access methods ###
def inverse_kinematics(self,
pose: np.ndarray,
seed: np.ndarray = None,
attempts: int = 0,
verbose: bool = True) -> np.ndarray:
"""
return a joint configuration for a given Cartesian pose of the end-effector
(ee_link) if any.
Parameters
----------
pose :
Cartesian pose of the end-effector defined as ee_link
seed : optional
if given, attempt to return a joint configuration closer to the seed
attempts : int, optional
number of attempts to find a IK solution. It may be useful for sample
based solvers such as TRAC-IK. It would not change the result of an
analytical solvers such as IKFast.
verbose : bool, optional
print a warning message when IK solutions are not found
Returns
-------
res : ndarray
Joint configuration if any or None
Raises
------
ValueError
If the rot_type is different from 'quaternion' or 'euler'
"""
q_guess_ = seed if seed is not None else self.joint_angles()
if self.ik_solver == IKSolverType.IKFAST:
# TODO: transform pose to the default tip used by IKFast (tool0)
ik = self.arm_ikfast.inverse(pose, q_guess=q_guess_)
elif self.ik_solver == IKSolverType.TRAC_IK:
ik = self.trac_ik.get_ik(q_guess_, *pose)
elif self.ik_solver == IKSolverType.KDL:
ik = self.kdl.inverse_kinematics(pose[:3], pose[3:], seed=q_guess_)
if ik is None:
if attempts > 0:
return self.inverse_kinematics(pose, seed, attempts-1)
if verbose:
rospy.logwarn(f"{self.ik_solver}: solution not found!")
raise InverseKinematicsException(f"{self.ik_solver}: solution not found!")
return ik
def end_effector(self,
joint_angles=None,
rot_type='quaternion',
tip_link=None) -> np.ndarray:
"""
Return the Cartesian pose of the end-effector in the robot base frame (base_link).
Parameters
----------
joint_angles : ndarray, optional
If not given, the current joint configuration will be used.
If provided, the joint configuration is expected in the order given by constants.JOINT_ORDER
rot_type : str, optional
Rotation representation to be returned.
Valid types "quaternion" or "euler"
tip_link : str, optional
Return the Cartesian pose of the tip_link if provided.
Otherwise, use the default ee_link
Returns
-------
res : ndarray
The Cartesian pose in the form of
quaternion: [x, y, z, aw, ax, ay, az] or
euler: [x, y, z, roll, pitch, yaw]
in radians.
Raises
------
ValueError
If the rot_type is different from 'quaternion' or 'euler'
"""
joint_angles = self.joint_angles() if joint_angles is None else joint_angles
if rot_type == 'quaternion':
# forward kinematics
return self.kdl.forward(joint_angles, tip_link)
elif rot_type == 'euler':
x = self.end_effector(joint_angles, tip_link=tip_link)
euler = np.array(transformations.euler_from_quaternion(x[3:], axes='sxyz'))
return np.concatenate((x[:3], euler))
elif rot_type == 'ortho6':
x = self.end_effector(joint_angles, tip_link=tip_link)
ortho6 = np.array(transformations.ortho6_from_quaternion(x[3:]))
return np.concatenate((x[:3], ortho6))
else:
raise ValueError("Rotation Type not supported", rot_type)
def joint_angle(self, joint: str) -> float:
"""
Return the requested joint angle in radians.
"""
joint_idx = self.joint_traj_controller.valid_joint_names.index(joint)
return self.joint_traj_controller.get_joint_positions()[joint_idx]
def joint_angles(self) -> np.ndarray:
"""
Returns the current joint positions in radians and
in the order given by constants.JOINT_ORDER.
"""
return self.joint_traj_controller.get_joint_positions()
def joint_velocity(self, joint: str) -> float:
"""
Return the requested joint velocity in radians/secs.
"""
joint_idx = self.joint_traj_controller.valid_joint_names.index(joint)
return self.joint_traj_controller.get_joint_velocities()[joint_idx]
def joint_velocities(self) -> np.ndarray:
"""
Returns the current joint velocities.
"""
return self.joint_traj_controller.get_joint_velocities()
def joint_effort(self, joint: str) -> float:
"""
Return the requested joint effort.
"""
joint_idx = self.joint_traj_controller.valid_joint_names.index(joint)
return self.joint_traj_controller.get_joint_efforts()[joint_idx]
def joint_efforts(self) -> np.ndarray:
"""
Returns the current joint efforts.
"""
return self.joint_traj_controller.get_joint_efforts()
def get_wrench_history(self, hist_size=24, hand_frame_control=False):
if self.current_ft_value is None:
raise Exception("FT Sensor not initialized")
ft_hist = np.array(self.wrench_queue)[:hist_size]
if hand_frame_control:
q_hist = self.joint_traj_controller.get_joint_positions_hist()[:hist_size]
poses_hist = [self.end_effector(q, tip_link=self.ee_link) for q in q_hist]
wrench_hist = [spalg.convert_wrench(wft, p).tolist() for p, wft in zip(poses_hist, ft_hist)]
else:
wrench_hist = ft_hist
return np.array(wrench_hist)
def get_wrench(self,
base_frame_control=False,
hand_frame_control=False) -> np.ndarray:
"""
Returns the wrench (force/torque) in task-space.
By default, return the wrench as read from the sensor topic.
Parameters
----------
base_frame_control : bool, optional
If True, returns the wrench with respect to the robot base frame
hand_frame_control : bool, optional
If True, returns the wrench with respect to the end-effector frame
If both base_frame_control and hand_frame_control are set to True.
the former is considered.
Returns
-------
res : np.ndarray
Returns the wrench in the requested frame
"""
if self.current_ft_value is None:
# No values have been received from sensor's topic
return np.zeros(6)
wrench_force = self.current_ft_value
if not hand_frame_control and not base_frame_control:
return wrench_force
if base_frame_control:
# Transform force/torque from sensor to robot base frame
transform = self.end_effector(tip_link=self.joint_names_prefix + "wrist_3_link")
ee_wrench_force = spalg.convert_wrench(wrench_force, transform)
return ee_wrench_force
else:
# Transform force/torque from sensor to end effector frame
transform = self.end_effector(tip_link=self.ee_link)
ee_wrench_force = spalg.convert_wrench(wrench_force, transform)
return ee_wrench_force
### Control Methods ###
def set_joint_positions(self,
target_time: float,
positions: np.ndarray,
velocities: np.ndarray = None,
accelerations: np.ndarray = None,
wait: bool = False) -> ExecutionResult:
"""
Run the joint trajectory controller towards a single waypoint starting now.
Parameters
----------
target_time : float
time at which target joint should be reach. It can be understood as the
duration of the trajectory.
positions : numpy.ndarray
target joint configuration in the order given by constants.JOINT_ORDER
velocities : numpy.ndarray, optional
target joint velocities
accelerations : numpy.ndarray, optional
target joint accelerations
wait : bool, optional
whether to block code execution until the trajectory is completed or not.
Returns
-------
res : bool
True if the trajectory is succesful when waiting for the execution to be
completed. Otherwise returns true if the trajectory was started.
"""
self.joint_traj_controller.add_point(positions=positions,
velocities=velocities,
accelerations=accelerations,
target_time=target_time)
if wait:
self.joint_traj_controller.start(delay=0, wait=True)
else:
self.joint_traj_controller.start_no_action_server()
# Always clear the trajectory goal
self.joint_traj_controller.clear_points()
if wait:
res = self.joint_traj_controller.get_result()
return ExecutionResult.DONE if res.error_code == 0 else ExecutionResult.CONTROLLER_FAILED
return ExecutionResult.DONE
def set_joint_trajectory(self,
target_time: float,
trajectory: np.ndarray,
velocities: np.ndarray = None,
accelerations: np.ndarray = None) -> ExecutionResult:
"""
Start the joint trajectory controller with a multi-waypoint trajectory.
Parameters
----------
target_time : float
time at which target joint should be reach. It can be understood as the
duration of the trajectory.
positions : 2-D numpy.ndarray
list of target joint configuration for each waypoint in the order given by constants.JOINT_ORDER
velocities : 2-D numpy.ndarray, optional
list of target joint velocities for each waypoint
accelerations : 2-D numpy.ndarray, optional
list of target joint accelerations for each waypoint
wait : bool, optional
whether to block code execution until the trajectory is completed or not.
Returns
-------
res : bool
True if the trajectory is succesfully executed.
"""
dt = target_time/len(trajectory)
for i, q in enumerate(trajectory):
self.joint_traj_controller.add_point(positions=q,
target_time=(i+1) * dt,
velocities=velocities,
accelerations=accelerations)
self.joint_traj_controller.start(delay=0, wait=True)
self.joint_traj_controller.clear_points()
res = self.joint_traj_controller.get_result()
return ExecutionResult.DONE if res.error_code == 0 else ExecutionResult.CONTROLLER_FAILED
def set_target_pose(self,
target_time: float,
pose: np.ndarray,
wait: bool = False) -> ExecutionResult:
"""
Reach a given target cartesian pose with the end-effector (ee_link)
Parameters
----------
target_time : float
time at which target joint should be reach. It can be understood as the
duration of the trajectory.
pose : numpy.ndarray
Cartesian target pose. Only the quaternion representation is supported
in the form: [x, y, z, aw, ax, ay, az]
wait : bool, optional
whether to block code execution until the trajectory is completed or not.
Returns
-------
res : bool
True if the trajectory is succesfully executed.
"""
q = self.inverse_kinematics(pose)
if q is None:
rospy.logdebug("IK not found")
raise InverseKinematicsException("IK solver failed to find a solution")
else:
return self.set_joint_positions(positions=q, target_time=target_time, wait=wait)
def set_pose_trajectory(self,
target_time: float,
trajectory: np.ndarray) -> ExecutionResult:
"""
Reach a given target cartesian pose with the end-effector (ee_link)
Parameters
----------
target_time : float
time at which target joint should be reach. It can be understood as the
duration of the trajectory.
pose : numpy.ndarray
Cartesian target pose. Only the quaternion representation is supported
in the form: [x, y, z, aw, ax, ay, az]
wait : bool, optional
whether to block code execution until the trajectory is completed or not.
Returns
-------
res : bool or str
True if the trajectory is succesfully executed.
If the IK solver fails, return "ik_not_found"
"""
joint_trajectory = []
previous_q = self.joint_angles()
for i, pose in enumerate(trajectory):
q = self.inverse_kinematics(pose, seed=previous_q)
if q is None:
raise InverseKinematicsException("IK solver failed to find a solution")
previous_q = q
joint_trajectory.append(q)
return self.set_joint_trajectory(trajectory=joint_trajectory, target_time=target_time)
def move_relative(self,
target_time: float,
transformation: np.array,
relative_to_tcp: bool = True,
wait: bool = True) -> ExecutionResult:
"""
Move end-effector (ee_link) relative to its current position
Parameters
----------
target_time : float
time at which target joint should be reach. It can be understood as the
duration of the trajectory.
pose : numpy.ndarray
Cartesian target pose. Only the quaternion representation is supported
in the form: [x, y, z, aw, ax, ay, az]
relative_to_tcp : bool, optional
if True, consider the current position relative to the end-effector frame.
if False, consider the current position relative to the robot base frame.
wait : bool, optional
whether to block code execution until the trajectory is completed or not.
Returns
-------
res : bool or str
True if the trajectory is succesfully executed.
If the IK solver fails, return "ik_not_found"
"""
new_pose = transformations.transform_pose(self.end_effector(), transformation, rotated_frame=relative_to_tcp)
return self.set_target_pose(pose=new_pose, target_time=target_time, wait=wait)
### FT sensor control ###
def zero_ft_sensor(self):
"""
Reset force-torque sensor readings to zeros.
"""
if not rospy.has_param("use_gazebo_sim"):
# First try to zero FT from ur_driver
self._zero_ft()
rospy.sleep(0.5)
# Then update filtered one
self._zero_ft_filtered()
def set_ft_filtering(self, active=True):
"""
Enable/disable a low-pass filter.
If active, the readings returned from self.get_wrench will have been filtered.
otherwise, the raw data from the sensor's topic will be returned.
The filtering is done in an external topic. See scripts/ft_filter.py
"""
self._ft_filtered(active)
================================================
FILE: ur_control/src/ur_control/compliance_controller.py
================================================
# Copyright (c) 2018-2021, Cristian Beltran. All rights reserved.
#
# Cristian Beltran and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto. Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from Cristian Beltran is strictly prohibited.
import rospy
import numpy as np
import types
from ur_control.arm import Arm
from ur_control import transformations, spalg, utils
from ur_control.constants import ExecutionResult
# Returns the new average
# after including x
def getAvg(prev_avg, x, n):
return ((prev_avg *
n + x) /
(n + 1))
class CompliantController(Arm):
def __init__(self,
model,
**kwargs):
""" Compliant controllertrajectory_time_compensation
"""
Arm.__init__(self, **kwargs)
self.model = model
# read publish rate if it does exist, otherwise set publish rate
js_rate = utils.read_parameter(self.ns + 'joint_state_controller/publish_rate', 500.0)
self.rate = rospy.Rate(js_rate)
def set_hybrid_control_trajectory(self, trajectory, max_force_torque, timeout=5.0,
stop_on_target_force=False, termination_criteria=None,
displacement_epsilon=0.002, check_displacement_time=2.0,
verbose=True, debug=False, time_compensation=True):
""" Move the robot according to a hybrid controller model
trajectory: array[array[7]] or array[7], can define a single target pose or a trajectory of multiple poses.
model: force control model, see hybrid_controller.py
max_force_torque: array[6], max force/torque allowed before stopping controller
timeout: float, maximum duration of controller's operation
stop_on_target_force: bool: stop once the model's target force has been achieved, stopping controller when all non-zero target forces/torques are reached
termination_criteria: lambda/function, special termination criteria based on current pose of the robot w.r.t the robot's base
displacement_epsilon: float, minimum displacement necessary to consider the robot in standby
check_displacement_time: float, time interval to check whether the displacement has been larger than displacement_epsilon
"""
reduced_speed = np.deg2rad([100, 100, 100, 250, 250, 250])
xb = self.end_effector()
failure_counter = 0
ptp_index = 0
q_last = self.joint_angles()
trajectory_time_compensation = self.model.dt * 10. if time_compensation else 0.0 # Hyperparameter
if trajectory.ndim == 1: # just one point
ptp_timeout = timeout
self.model.set_goals(position=trajectory)
else: # trajectory
ptp_timeout = timeout / float(len(trajectory)) - trajectory_time_compensation
self.model.set_goals(position=trajectory[ptp_index])
log = {ExecutionResult.SPEED_LIMIT_EXCEEDED: 0, ExecutionResult.IK_NOT_FOUND: 0}
result = ExecutionResult.DONE
standby_timer = rospy.get_time()
standby_last_pose = self.end_effector()
standby = False
if debug:
avg_step_time = 0.0
step_num = 0
# Timeout for motion
initime = rospy.get_time()
sub_inittime = rospy.get_time()
while not rospy.is_shutdown() \
and (rospy.get_time() - initime) < timeout:
if debug:
start_time = rospy.get_time()
# Transform wrench to the base_link frame
Wb = self.get_wrench(base_frame_control=True)
# Current position in task-space
xb = self.end_effector()
if termination_criteria is not None:
assert isinstance(termination_criteria, types.LambdaType), "Invalid termination criteria, expecting lambda/function with one argument[current pose array[7]]"
if termination_criteria(xb, standby):
rospy.loginfo("Termination criteria returned True, stopping force control")
result = ExecutionResult.TERMINATION_CRITERIA
break
if (rospy.get_time() - sub_inittime) > ptp_timeout:
sub_inittime = rospy.get_time()
ptp_index += 1
if ptp_index >= len(trajectory):
self.model.set_goals(position=trajectory[-1])
elif not trajectory.ndim == 1: # For some reason the timeout validation is not robust enough
self.model.set_goals(position=trajectory[ptp_index])
Fb = -1 * Wb # Move in the opposite direction of the force
if stop_on_target_force and np.all(np.abs(Fb)[self.model.target_force != 0] > np.abs(self.model.target_force)[self.model.target_force != 0]):
rospy.loginfo('Target F/T reached {}'.format(np.round(Wb, 3)) + ' Stopping!')
self.set_target_pose(pose=xb, target_time=self.model.dt)
result = ExecutionResult.STOP_ON_TARGET_FORCE
break
# Safety limits: max force
if np.any(np.abs(Wb) > max_force_torque):
rospy.logerr('Maximum force/torque exceeded {}'.format(np.round(Wb, 3)))
self.set_target_pose(pose=xb, target_time=self.model.dt)
result = ExecutionResult.FORCE_TORQUE_EXCEEDED
break
# Current Force in task-space
dxf = self.model.control_position_orientation(Fb, xb) # angular velocity
xc = transformations.pose_from_angular_velocity(xb, dxf, dt=self.model.dt)
# Avoid extra acceleration when a point failed due to IK or other violation
# So, this corrects the allowed time for the next point
dt = self.model.dt * (failure_counter+1)
result = self._actuate(xc, dt, q_last, reduced_speed)
if result != ExecutionResult.DONE:
failure_counter += 1
if result == ExecutionResult.IK_NOT_FOUND:
log[ExecutionResult.IK_NOT_FOUND] += 1
if result == ExecutionResult.SPEED_LIMIT_EXCEEDED:
log[ExecutionResult.SPEED_LIMIT_EXCEEDED] += 1
continue # Don't wait since there is not motion
else:
failure_counter = 0
q_last = self.joint_angles()
# Compensate the time allocated to the next command when there are failures
# Especially important for following a motion trajectory
for _ in range(failure_counter+1):
self.rate.sleep()
standby_time = (rospy.get_time() - standby_timer)
if standby_time > check_displacement_time:
displacement_dt = np.linalg.norm(standby_last_pose[:3] - self.end_effector()[:3])
standby = displacement_dt < displacement_epsilon
if standby:
rospy.logwarn("No more than %s displacement in the last %s seconds" % (round(displacement_dt, 6), check_displacement_time))
last_pose = self.end_effector()
standby_timer = rospy.get_time()
standby_last_pose = self.end_effector()
if debug:
step_time = rospy.get_time() - start_time
avg_step_time = step_time if avg_step_time == 0 else getAvg(avg_step_time, step_time, step_num)
step_num += 1
if verbose:
rospy.logwarn("Total # of commands ignored: %s" % log)
return result
def _actuate(self, pose, dt, q_last, reduced_speed, attempts=5):
"""
Evaluate IK solution several times if it fails.
Similarly, evaluate that the IK solution is viable
"""
result = None
q = self.inverse_kinematics(pose, attempts=0, verbose=False)
if q is None:
if attempts > 0:
return self._actuate(pose, dt, q_last, reduced_speed, attempts-1)
rospy.logwarn("IK not found")
result = ExecutionResult.IK_NOT_FOUND
else:
q_speed = (q_last - q)/dt
if np.any(np.abs(q_speed) > reduced_speed):
if attempts > 0:
return self._actuate(pose, dt, q_last, reduced_speed, attempts-1)
rospy.logwarn_once("Exceeded reduced max speed %s deg/s, Ignoring command" % np.round(np.rad2deg(q_speed), 0))
result = ExecutionResult.SPEED_LIMIT_EXCEEDED
else:
result = self.set_joint_positions(positions=q, target_time=dt)
self.rate.sleep()
return result
# TODO(cambel): organize this code to avoid this repetition of code
def set_hybrid_control(self, model, max_force_torque, timeout=5.0, stop_on_target_force=False):
""" Move the robot according to a hybrid controller model"""
reduced_speed = np.deg2rad([100, 100, 100, 150, 150, 150])
q_last = self.joint_angles()
# Timeout for motion
initime = rospy.get_time()
xb = self.end_effector()
failure_counter = 0
while not rospy.is_shutdown() \
and (rospy.get_time() - initime) < timeout:
# Transform wrench to the base_link frame
Wb = self.get_wrench()
# Current Force in task-space
Fb = -1 * Wb
# Safety limits: max force
if np.any(np.abs(Fb) > max_force_torque):
rospy.logerr('Maximum force/torque exceeded {}'.format(np.round(Wb, 3)))
self.set_target_pose(pose=xb, target_time=model.dt)
return ExecutionResult.FORCE_TORQUE_EXCEEDED
if stop_on_target_force and np.any(np.abs(Fb)[model.target_force != 0] > model.target_force[model.target_force != 0]):
rospy.loginfo('Target F/T reached {}'.format(np.round(Wb, 3)) + ' Stopping!')
self.set_target_pose(pose=xb, target_time=model.dt)
return ExecutionResult.STOP_ON_TARGET_FORCE
# Current position in task-space
xb = self.end_effector()
dxf = model.control_position_orientation(Fb, xb) # angular velocity
# Limit linear/angular velocity
dxf[:3] = np.clip(dxf[:3], -0.5, 0.5)
dxf[3:] = np.clip(dxf[3:], -5., 5.)
xc = transformations.pose_from_angular_velocity(xb, dxf, dt=model.dt)
# Avoid extra acceleration when a point failed due to IK or other violation
# So, this corrects the allowed time for the next point
dt = model.dt * (failure_counter+1)
q = self.inverse_kinematics(xc)
if q is None:
rospy.logwarn("IK not found")
result = ExecutionResult.IK_NOT_FOUND
else:
q_speed = (q_last - q)/dt
if np.any(np.abs(q_speed) > reduced_speed):
rospy.logwarn("Exceeded reduced max speed %s deg/s, Ignoring command" % np.round(np.rad2deg(q_speed), 0))
result = ExecutionResult.SPEED_LIMIT_EXCEEDED
else:
result = self.set_joint_positions(positions=q, target_time=dt)
if result != ExecutionResult.DONE:
failure_counter += 1
continue # Don't wait since there is not motion
else:
failure_counter = 0
# Compensate the time allocated to the next command when there are failures
for _ in range(failure_counter+1):
self.rate.sleep()
q_last = self.joint_angles()
return ExecutionResult.DONE
================================================
FILE: ur_control/src/ur_control/constants.py
================================================
from enum import Enum
class IKSolverType(Enum):
KDL = 'kdl'
TRAC_IK = 'Trac-IK'
IKFAST = 'IKFast'
class GripperType(Enum):
GENERIC = 'generic'
ROBOTIQ = 'robotiq'
class ExecutionResult(Enum):
DONE = 'success'
CONTROLLER_FAILED = 'controller_failed'
FORCE_TORQUE_EXCEEDED = 'force_exceeded'
STOP_ON_TARGET_FORCE = 'stop_on_target_force'
IK_NOT_FOUND = 'ik_not_found'
SPEED_LIMIT_EXCEEDED = 'speed_limit_exceeded'
TERMINATION_CRITERIA = 'termination_criteria_achieved'
CARTESIAN_COMPLIANCE_CONTROLLER = 'cartesian_compliance_controller'
JOINT_TRAJECTORY_CONTROLLER = 'scaled_pos_joint_traj_controller'
JOINT_SUBSCRIBER = '/arm_controller/state'
JOINT_STATE_SUBSCRIBER = 'joint_states'
FT_SUBSCRIBER = 'wrench'
# Set constants for joints
SHOULDER_PAN_JOINT = 'shoulder_pan_joint'
SHOULDER_LIFT_JOINT = 'shoulder_lift_joint'
ELBOW_JOINT = 'elbow_joint'
WRIST_1_JOINT = 'wrist_1_joint'
WRIST_2_JOINT = 'wrist_2_joint'
WRIST_3_JOINT = 'wrist_3_joint'
BASE_LINK = 'base_link'
EE_LINK = 'tool0'
FT_LINK = 'tool0'
JOINT_ORDER = [
SHOULDER_PAN_JOINT, SHOULDER_LIFT_JOINT, ELBOW_JOINT, WRIST_1_JOINT,
WRIST_2_JOINT, WRIST_3_JOINT
]
def get_arm_joint_names(prefix):
return [prefix + joint for joint in JOINT_ORDER]
================================================
FILE: ur_control/src/ur_control/controllers.py
================================================
#!/usr/bin/env python
import actionlib
import copy
import collections
import rospy
from ur_control import utils, constants
import numpy as np
from std_msgs.msg import Float64
from controller_manager_msgs.srv import ListControllers
# Joint trajectory action
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal, FollowJointTrajectoryResult
class JointControllerBase(object):
"""
Base class for the Joint Position Controllers. It subscribes to the C{joint_states} topic by default.
"""
def __init__(self, namespace, timeout, joint_names=None):
"""
JointControllerBase constructor. It subscribes to the C{joint_states} topic and informs after
successfully reading a message from the topic.
@type namespace: string
@param namespace: Override ROS namespace manually. Useful when controlling several robots
@type timeout: float
@param timeout: Time in seconds that will wait for the controller
from the same node.
"""
self.valid_joint_names = constants.JOINT_ORDER if joint_names is None else joint_names
self.ns = utils.solve_namespace(namespace)
self._jnt_positions_hist = collections.deque(maxlen=24)
# Set-up publishers/subscribers
self._js_sub = rospy.Subscriber('joint_states', JointState, self.joint_states_cb, queue_size=1)
retry = False
rospy.logdebug('Waiting for [%sjoint_states] topic' % self.ns)
start_time = rospy.get_time()
while not hasattr(self, '_joint_names'):
if (rospy.get_time() - start_time) > timeout and not retry:
# Re-try with namespace
self._js_sub = rospy.Subscriber('%sjoint_states' % self.ns, JointState, self.joint_states_cb, queue_size=1)
start_time = rospy.get_time()
retry = True
continue
elif (rospy.get_time() - start_time) > timeout and retry:
rospy.logerr('Timed out waiting for joint_states topic')
return
rospy.sleep(0.01)
if rospy.is_shutdown():
return
if rospy.has_param(f"{self.ns}joint_state_controller/publish_rate"):
self.rate = utils.read_parameter(f"{self.ns}joint_state_controller/publish_rate", 500)
elif rospy.has_param(f"/joint_state_controller/publish_rate"):
self.rate = utils.read_parameter(f"/joint_state_controller/publish_rate", 500)
self._num_joints = len(self._joint_names)
rospy.logdebug('Topic [%sjoint_states] found' % self.ns)
def disconnect(self):
"""
Disconnects from the joint_states topic. Useful to ligthen the use of system resources.
"""
self._js_sub.unregister()
def get_joint_efforts(self):
"""
Returns the current joint efforts of the UR robot.
@rtype: numpy.ndarray
@return: Current joint efforts of the UR robot.
"""
return np.array(self._current_jnt_efforts)
def get_joint_positions(self):
"""
Returns the current joint positions of the UR robot.
@rtype: numpy.ndarray
@return: Current joint positions of the UR robot.
"""
return np.array(self._current_jnt_positions)
def get_joint_positions_hist(self):
"""
Returns the current joint positions of the UR robot.
@rtype: numpy.ndarray
@return: Current joint positions of the UR robot.
"""
return list(self._jnt_positions_hist)
def get_joint_velocities(self):
"""
Returns the current joint velocities of the UR robot.
@rtype: numpy.ndarray
@return: Current joint velocities of the UR robot.
"""
return np.array(self._current_jnt_velocities)
def joint_states_cb(self, msg):
"""
Callback executed every time a message is publish in the C{joint_states} topic.
@type msg: sensor_msgs/JointState
@param msg: The JointState message published by the RT hardware interface.
"""
position = []
velocity = []
effort = []
name = []
for joint_name in self.valid_joint_names:
if joint_name in msg.name:
idx = msg.name.index(joint_name)
name.append(msg.name[idx])
if msg.effort:
effort.append(msg.effort[idx])
if msg.velocity:
velocity.append(msg.velocity[idx])
position.append(msg.position[idx])
if set(name) == set(self.valid_joint_names):
self._current_jnt_positions = np.array(position)
self._jnt_positions_hist.append(self._current_jnt_positions)
self._current_jnt_velocities = np.array(velocity)
self._current_jnt_efforts = np.array(effort)
self._joint_names = list(name)
class JointPositionController(JointControllerBase):
"""
Interface class to control the UR robot using a Joint Position Control approach.
If you C{set_joint_positions} to a value very far away from the current robot position,
it will move at its maximum speed/acceleration and will even move the base of the robot, so, B{use with caution}.
"""
def __init__(self, namespace='', timeout=5.0, joint_names=None):
"""
C{JointPositionController} constructor. It creates the required publishers for controlling
the UR robot. Given that it inherits from C{JointControllerBase} it subscribes
to C{joint_states} by default.
@type namespace: string
@param namespace: Override ROS namespace manually. Useful when controlling several robots
from the same node.
@type timeout: float
@param timeout: Time in seconds that will wait for the controller
"""
super(JointPositionController, self).__init__(namespace, timeout=timeout, joint_names=joint_names)
if not hasattr(self, '_joint_names'):
raise rospy.ROSException('JointPositionController timed out waiting joint_states topic: {0}'.format(namespace))
self._cmd_pub = dict()
for joint in self._joint_names:
print(('%s%scommand' % (self.ns, joint)))
self._cmd_pub[joint] = rospy.Publisher('%s%scommand' % (self.ns, joint), Float64, queue_size=3)
# Wait for the joint position controllers
controller_list_srv = self.ns + 'controller_manager/list_controllers'
rospy.logdebug('Waiting for the joint position controllers...')
rospy.wait_for_service(controller_list_srv, timeout=timeout)
list_controllers = rospy.ServiceProxy(controller_list_srv, ListControllers)
expected_controllers = (joint_names if joint_names is not None else constants.JOINT_ORDER)
start_time = rospy.get_time()
while not rospy.is_shutdown():
if (rospy.get_time() - start_time) > timeout:
raise rospy.ROSException('JointPositionController timed out waiting for the controller_manager: {0}'.format(namespace))
rospy.sleep(0.01)
found = 0
try:
res = list_controllers()
for state in res.controller:
if state.name in expected_controllers:
found += 1
except:
pass
if found == len(expected_controllers):
break
rospy.loginfo('JointPositionController initialized. ns: {0}'.format(namespace))
def set_joint_positions(self, jnt_positions):
"""
Sets the joint positions of the robot. The values are send directly to the robot. If the goal
is too far away from the current robot position, it will move at its maximum speed/acceleration
and will even move the base of the robot, so, B{use with caution}.
@type jnt_positions: list
@param jnt_positions: Joint positions command.
"""
if not self.valid_jnt_command(jnt_positions):
rospy.logwarn('A valid joint positions command should have %d elements' % (self._num_joints))
return
# Publish the point for each joint
for name, q in zip(self._joint_names, jnt_positions):
try:
self._cmd_pub[name].publish(q)
except:
print("some error")
pass
def valid_jnt_command(self, command):
"""
It validates that the length of a joint command is equal to the number of joints
@type command: list
@param command: Joint command to be validated
@rtype: bool
@return: True if the joint command is valid
"""
return (len(command) == self._num_joints)
class JointTrajectoryController(JointControllerBase):
"""
This class creates a C{SimpleActionClient} that connects to the
C{trajectory_controller/follow_joint_trajectory} action server. Using this
interface you can control the robot by adding points to the trajectory. Each point
requires the goal position and the goal time. The velocity and acceleration are optional.
The controller is templated to work with multiple trajectory representations. By default,
a B{linear} interpolator is provided, but it's possible to support other representations.
The interpolator uses the following interpolation strategies depending on the waypoint
specification:
- B{Linear}: Only position is specified. Guarantees continuity at the position level.
B{Discouraged} because it yields trajectories with discontinuous velocities at the waypoints.
- B{Cubic}: Position and velocity are specified. Guarantees continuity at the velocity level.
- B{Quintic}: Position, velocity and acceleration are specified: Guarantees continuity at
the acceleration level.
"""
def __init__(self, publisher_name='arm_controller', namespace='', timeout=5.0, joint_names=None):
"""
JointTrajectoryController constructor. It creates the C{SimpleActionClient}.
@type namespace: string
@param namespace: Override ROS namespace manually. Useful when controlling several
robots from the same node.
@type timeout: float
@param timeout: Time in seconds that will wait for the controller
"""
super(JointTrajectoryController, self).__init__(namespace, timeout=timeout, joint_names=joint_names)
trajectory_publisher_topic = self.ns + publisher_name + '/command'
self.trajectory_pub = rospy.Publisher(trajectory_publisher_topic, JointTrajectory, queue_size=10)
action_server = self.ns + publisher_name + '/follow_joint_trajectory'
self._client = actionlib.SimpleActionClient(action_server, FollowJointTrajectoryAction)
self._goal = FollowJointTrajectoryGoal()
rospy.logdebug('Waiting for [%s] action server' % action_server)
topics = [item for sublist in rospy.get_published_topics() for item in sublist]
if action_server+"/goal" not in topics:
raise rospy.ROSException("Action server not found")
server_up = self._client.wait_for_server(timeout=rospy.Duration(timeout))
if not server_up:
rospy.logerr('Timed out waiting for Joint Trajectory'
' Action Server to connect. Start the action server'
' before running this node.')
raise rospy.ROSException('JointTrajectoryController timed out: {0}'.format(action_server))
rospy.logdebug('Successfully connected to [%s]' % action_server)
# Get a copy of joint_names
if not hasattr(self, '_joint_names'):
raise rospy.ROSException('JointTrajectoryController timed out waiting joint_states topic: {0}'.format(self.ns))
self._goal.trajectory.joint_names = copy.deepcopy(self._joint_names)
rospy.loginfo('JointTrajectoryController initialized. ns: {0}'.format(self.ns))
def add_point(self, target_time, positions, velocities=None, accelerations=None):
"""
Adds a point to the trajectory. Each point must be specified by the goal position and
the goal time. The velocity and acceleration are optional.
@type positions: list
@param positions: The goal position in the joint space
@type target_time: float
@param target_time: The time B{from start} when the robot should arrive at the goal position.
@type velocities: list
@param velocities: The velocity of arrival at the goal position. If not given zero
velocity is assumed.
@type accelerations: list
@param accelerations: The acceleration of arrival at the goal position. If not given
zero acceleration is assumed.
"""
point = JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if type(velocities) == type(None):
point.velocities = [0] * self._num_joints
else:
point.velocities = copy.deepcopy(velocities)
if type(accelerations) == type(None):
point.accelerations = [0] * self._num_joints
else:
point.accelerations = copy.deepcopy(accelerations)
point.time_from_start = rospy.Duration(target_time)
self._goal.trajectory.points.append(point)
def clear_points(self):
"""
Clear all points in the trajectory.
"""
self._goal.trajectory.points = []
def get_num_points(self):
"""
Returns the number of points currently added to the trajectory
@rtype: int
@return: Number of points currently added to the trajectory
"""
return len(self._goal.trajectory.points)
def get_result(self) -> FollowJointTrajectoryResult:
"""
Returns the result B{after} the execution of the trajectory. Possible values:
- FollowJointTrajectoryResult.SUCCESSFUL = 0
- FollowJointTrajectoryResult.INVALID_GOAL = -1
- FollowJointTrajectoryResult.INVALID_JOINTS = -2
- FollowJointTrajectoryResult.OLD_HEADER_TIMESTAMP = -3
- FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED = -4
- FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED = -5
@rtype: int
@return: result B{after} the execution of the trajectory
"""
return self._client.get_result()
def get_state(self):
"""
Returns the status B{during} the execution of the trajectory. Possible values:
- GoalStatus.PENDING=0
- GoalStatus.ACTIVE=1
- GoalStatus.PREEMPTED=2
- GoalStatus.SUCCEEDED=3
- GoalStatus.ABORTED=4
- GoalStatus.REJECTED=5
- GoalStatus.PREEMPTING=6
- GoalStatus.RECALLING=7
- GoalStatus.RECALLED=8
- GoalStatus.LOST=9
@rtype: int
@return: result B{after} the execution of the trajectory
"""
return self._client.get_state()
def set_trajectory(self, trajectory):
"""
Sets the goal trajectory directly. B{It only copies} the C{trajectory.points} field.
@type trajectory: trajectory_msgs/JointTrajectory
@param trajectory: The goal trajectory
"""
self._goal.trajectory.points = copy.deepcopy(trajectory.points)
def start(self, delay=0.1, wait=False):
"""
Starts the trajectory. It sends the C{FollowJointTrajectoryGoal} to the action server.
@type delay: float
@param delay: Delay (in seconds) before executing the trajectory
"""
num_points = len(self._goal.trajectory.points)
rospy.logdebug('Executing Joint Trajectory with {0} points'.format(num_points))
start_time = rospy.Time() if delay == 0 else rospy.Time.now() + rospy.Duration(delay)
self._goal.trajectory.header.stamp = start_time
if wait:
self._client.send_goal_and_wait(self._goal)
else:
self._client.send_goal(self._goal)
def stop(self):
"""
Stops an active trajectory. If there is not active trajectory an error will be shown in the console.
"""
self._client.cancel_goal()
def wait(self, timeout=15.0):
"""
Waits synchronously (with a timeout) until the trajectory action server gives a result.
@type timeout: float
@param timeout: The amount of time we will wait
@rtype: bool
@return: True if the server connected in the allocated time. False on timeout
"""
return self._client.wait_for_result(timeout=rospy.Duration(timeout))
def start_no_action_server(self):
"""
Start the trajectory without expecting any feedback from the action server.
"""
self.trajectory_pub.publish(self._goal.trajectory)
================================================
FILE: ur_control/src/ur_control/controllers_connection.py
================================================
#!/usr/bin/env python
import rospy
from controller_manager_msgs.srv import SwitchController, SwitchControllerRequest, \
LoadController, UnloadController, ListControllers
from ur_control.utils import solve_namespace
class ControllersConnection():
def __init__(self, namespace=None):
self.controllers_list = []
self.ns = solve_namespace(namespace)[1:]
if namespace:
prefix = namespace + 'controller_manager/'
else:
prefix = '/controller_manager/'
# Only for osx sim
if rospy.has_param("use_gazebo_sim"):
prefix = '/controller_manager/'
self.switch_service = rospy.ServiceProxy(prefix + 'switch_controller', SwitchController)
self.load_service = rospy.ServiceProxy(prefix + 'load_controller', LoadController)
self.unload_service = rospy.ServiceProxy(prefix + 'unload_controller', UnloadController)
self.list_controllers_service = rospy.ServiceProxy(prefix + 'list_controllers', ListControllers)
self.list_controllers_service.wait_for_service(1.0)
def get_loaded_controllers(self):
self.controllers_list = []
try:
result = self.list_controllers_service()
for controller in result.controller:
self.controllers_list.append(controller.name)
except:
pass
def get_controller_state(self, controller_name):
try:
result = self.list_controllers_service()
for controller in result.controller:
if controller.name == controller_name:
return controller.state
except:
pass
rospy.logerr("Controller %s not found" % controller_name)
return None
def load_controllers(self, controllers_list):
self.get_loaded_controllers()
self.load_service.wait_for_service(0.1)
for controller in controllers_list:
if controller not in self.controllers_list:
result = self.load_service(controller)
rospy.loginfo('Loading controller %s. Result=%s' % (controller, result))
if result:
self.controllers_list.append(controller)
def unload_controllers(self, controllers_list):
try:
self.unload_service.wait_for_service(0.1)
for controller in controllers_list:
result = self.unload_service(controller)
rospy.loginfo('Unloading controller %s. Result=%s' % (controller, result))
if result:
self.controllers_list.remove(controller)
except Exception as e:
rospy.logerr("Unload controllers service call failed: %s" % e)
def check_controllers_state(self, on_controllers, off_controllers):
for c in on_controllers:
if self.get_controller_state(c) != "running":
return False
for c in off_controllers:
if self.get_controller_state(c) != "stopped":
return False
return True
def switch_controllers(self, controllers_on, controllers_off,
strictness=1):
"""
Give the controllers you want to switch on or off.
:param controllers_on: ["name_controller_1", "name_controller2",...,"name_controller_n"]
:param controllers_off: ["name_controller_1", "name_controller2",...,"name_controller_n"]
:return:
"""
if rospy.has_param("use_gazebo_sim"):
controllers_on = [self.ns + controller for controller in controllers_on]
controllers_off = [self.ns + controller for controller in controllers_off]
try:
self.switch_service.wait_for_service(0.1)
switch_request_object = SwitchControllerRequest()
switch_request_object.start_controllers = controllers_on
switch_request_object.stop_controllers = controllers_off
switch_request_object.strictness = strictness
if self.check_controllers_state(controllers_on, controllers_off):
return True
switch_result = self.switch_service(switch_request_object)
"""
[controller_manager_msgs/SwitchController]
int32 BEST_EFFORT=1
int32 STRICT=2
string[] start_controllers
string[] stop_controllers
int32 strictness
---
bool ok
"""
rospy.logdebug("Switch Result==>" + str(switch_result.ok))
if not switch_result.ok: # Return if service failed
return False
# Check that the controllers are running before returning
start_time = rospy.get_time()
while (rospy.get_time() - start_time) < 5.0 and not rospy.is_shutdown():
all_running = True
for controller in controllers_on:
if self.get_controller_state(controller) != "running":
all_running = False
break
if all_running:
break
return switch_result.ok
except Exception as e:
rospy.logerr("Switch controllers service call failed: %s" % e)
return None
def reset_controllers(self):
"""
We turn on and off the given controllers
:param controllers_reset: ["name_controller_1", "name_controller2",...,"name_controller_n"]
:return:
"""
reset_result = False
result_off_ok = self.switch_controllers(controllers_on=[], controllers_off=self.controllers_list)
rospy.logdebug("Deactivated Controllers")
if result_off_ok:
rospy.logdebug("Activating Controllers")
result_on_ok = self.switch_controllers(
controllers_on=self.controllers_list, controllers_off=[])
if result_on_ok:
rospy.logdebug("Controllers Reset==>" +
str(self.controllers_list))
reset_result = True
else:
rospy.logdebug("result_on_ok==>" + str(result_on_ok))
else:
rospy.logdebug("result_off_ok==>" + str(result_off_ok))
return reset_result
def update_controllers_list(self, new_controllers_list):
self.controllers_list = new_controllers_list
================================================
FILE: ur_control/src/ur_control/conversions.py
================================================
#! /usr/bin/env python
import numpy as np
from ur_control import transformations as tr
# Messages
from geometry_msgs.msg import (Point, Quaternion, Pose, PoseStamped, Vector3, Transform,
Wrench)
from sensor_msgs.msg import RegionOfInterest
import pyquaternion
# OpenRAVE types <--> Numpy types
def from_dict(transform_dict):
"""
Converts a dictionary with the fields C{rotation} and C{translation}
into a homogeneous transformation of type C{np.array}.
@type transform_dict: dict
@param transform_dict: The dictionary to be converted.
@rtype: np.array
@return: The resulting numpy array
"""
T = tr.rotation_matrix_from_quaternion(np.array(transform_dict['rotation']))
T[:3, 3] = np.array(transform_dict['translation'])
return T
# PyKDL types <--> Numpy types
def from_kdl_vector(vector):
"""
Converts a C{PyKDL.Vector} with fields into a numpy array.
@type vector: PyKDL.Vector
@param vector: The C{PyKDL.Vector} to be converted
@rtype: np.array
@return: The resulting numpy array
"""
return np.array([vector.x(), vector.y(), vector.z()])
def from_kdl_twist(twist):
"""
Converts a C{PyKDL.Twist} with fields into a numpy array.
@type twist: PyKDL.Twist
@param twist: The C{PyKDL.Twist} to be converted
@rtype: np.array
@return: The resulting numpy array
"""
array = np.zeros(6)
array[:3] = from_kdl_vector(twist.vel)
array[3:] = from_kdl_vector(twist.rot)
return array
# ROS types <--> Numpy types
def from_point(msg):
"""
Converts a C{geometry_msgs/Point} ROS message into a numpy array.
@type msg: geometry_msgs/Point
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
return from_vector3(msg)
def from_pose(msg):
"""
Converts a C{geometry_msgs/Pose} ROS message into a numpy array (Homogeneous transformation 4x4).
@type msg: geometry_msgs/Pose
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
T = tr.rotation_matrix_from_quaternion(from_quaternion(msg.orientation))
T[:3, 3] = from_point(msg.position)
return T
def from_pose_to_list(msg):
"""
Converts a C{geometry_msgs/Pose} ROS message into a numpy array (7 elements, xyz+xyzw).
@type msg: geometry_msgs/Pose
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
return np.concatenate([from_point(msg.position), from_quaternion(msg.orientation)])
def from_quaternion(msg):
"""
Converts a C{geometry_msgs/Quaternion} ROS message into a numpy array.
@type msg: geometry_msgs/Quaternion
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
return np.array([msg.x, msg.y, msg.z, msg.w], dtype=float)
def from_roi(msg):
top_left = np.array([msg.x_offset, msg.y_offset])
bottom_right = top_left + np.array([msg.width, msg.height])
return [top_left, bottom_right]
def from_transform(msg):
"""
Converts a C{geometry_msgs/Transform} ROS message into a numpy array.
@type msg: geometry_msgs/Transform
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
T = tr.rotation_matrix_from_quaternion(from_quaternion(msg.rotation))
T[:3, 3] = from_vector3(msg.translation)
return T
def from_vector3(msg):
"""
Converts a C{geometry_msgs/Vector3} ROS message into a numpy array.
@type msg: geometry_msgs/Vector3
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
return np.array([msg.x, msg.y, msg.z], dtype=float)
def from_wrench(msg):
"""
Converts a C{geometry_msgs/Wrench} ROS message into a numpy array.
@type msg: geometry_msgs/Wrench
@param msg: The ROS message to be converted
@rtype: np.array
@return: The resulting numpy array
"""
array = np.zeros(6)
array[:3] = from_vector3(msg.force)
array[3:] = from_vector3(msg.torque)
return array
def to_quaternion(array):
"""
Converts a numpy array into a C{geometry_msgs/Quaternion} ROS message.
@type array: np.array
@param array: The position as numpy array
@rtype: geometry_msgs/Quaternion
@return: The resulting ROS message
"""
return Quaternion(*array)
def to_point(array):
"""
Converts a numpy array into a C{geometry_msgs/Point} ROS message.
@type array: np.array
@param array: The position as numpy array
@rtype: geometry_msgs/Point
@return: The resulting ROS message
"""
return Point(*array)
def to_pose(T):
"""
Converts a homogeneous transformation (4x4) into a C{geometry_msgs/Pose} ROS message.
@type T: np.array
@param T: The homogeneous transformation
@rtype: geometry_msgs/Pose
@return: The resulting ROS message
"""
T = np.array(T, dtype=float)
if len(T) == 6:
pos = Point(*T[:3])
quat = Quaternion(*tr.quaternion_from_euler(*T[3:]))
elif len(T) == 7:
pos = Point(*T[:3])
quat = to_quaternion(T[3:])
else:
pos = Point(*T[:3, 3])
quat = Quaternion(*tr.rotation_matrix_from_quaternion(T))
return Pose(pos, quat)
def to_roi(top_left, bottom_right):
msg = RegionOfInterest()
msg.x_offset = round(top_left[0])
msg.y_offset = round(top_left[1])
msg.width = round(abs(bottom_right[0] - top_left[0]))
msg.height = round(abs(bottom_right[1] - top_left[1]))
return msg
def to_transform(T):
"""
Converts a homogeneous transformation (4x4) into a C{geometry_msgs/Transform}
ROS message.
@type T: np.array
@param T: The homogeneous transformation
@rtype: geometry_msgs/Transform
@return: The resulting ROS message
"""
if len(T) == 7:
translation = Vector3(*T[:3])
rotation = to_quaternion(T[3:])
else:
translation = Vector3(*T[:3, 3])
rotation = Quaternion(*tr.rotation_matrix_from_quaternion(T))
return Transform(translation, rotation)
def to_vector3(array):
"""
Converts a numpy array into a C{geometry_msgs/Vector3} ROS message.
@type array: np.array
@param array: The vector as numpy array
@rtype: geometry_msgs/Vector3
@return: The resulting ROS message
"""
return Vector3(*array)
def to_wrench(array):
"""
Converts a numpy array into a C{geometry_msgs/Wrench} ROS message.
@type array: np.array
@param array: The wrench as numpy array
@rtype: geometry_msgs/Wrench
@return: The resulting ROS message
"""
msg = Wrench()
msg.force = to_vector3(array[:3])
msg.torque = to_vector3(array[3:])
return msg
# RViz types <--> Numpy types
def from_rviz_vector(value, dtype=float):
"""
Converts a RViz property vector in the form C{X;Y;Z} into a numpy array.
@type value: str
@param value: The RViz property vector
@type dtype: type
@param dtype: The type of mapping to be done. Typically C{float} or C{int}.
@rtype: array
@return: The resulting numpy array
"""
strlst = value.split(';')
return np.array(list(map(dtype, strlst)))
def angleAxis_from_euler(euler):
roll, pitch, yaw = euler
yaw_matrix = np.matrix([[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
pitch_matrix = np.matrix([[np.cos(pitch), 0,
np.sin(pitch)], [0, 1, 0],
[-np.sin(pitch), 0,
np.cos(pitch)]])
roll_matrix = np.matrix([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]])
R = yaw_matrix * pitch_matrix * roll_matrix
theta = np.arccos(((R[0, 0] + R[1, 1] + R[2, 2]) - 1) / 2)
if theta == 0:
return [0., 0., 0.]
multi = 1 / (2 * np.sin(theta))
rx = multi * (R[2, 1] - R[1, 2]) * theta
ry = multi * (R[0, 2] - R[2, 0]) * theta
rz = multi * (R[1, 0] - R[0, 1]) * theta
return [rx, ry, rz]
def euler_transformation_matrix(euler):
""" Euler's transformation matrix """
r, p, y = euler
T = np.array([[1, 0, np.sin(p)], [0, np.cos(r), -np.sin(r) * np.cos(p)],
[0, np.sin(r), np.cos(r) * np.cos(p)]])
return T
def transform_end_effector(pose, extra_pose, rot_type='quaternion', inverse=False):
"""
Transform end effector pose
pose: current pose [x, y, z, ax, ay, az, w]
extra_pose: additional transformation [x, y, z, ax, ay, az, w]
matrix: if true: return (translation, rotation matrix)
else: return translation + quaternion list
"""
extra_translation = np.array(extra_pose[:3]).reshape(3, 1)
extra_rot = tr.vector_to_pyquaternion(extra_pose[3:]).rotation_matrix
c_trans = np.array(pose[:3]).reshape(3, 1)
c_rot = tr.vector_to_pyquaternion(pose[3:]).rotation_matrix
# BE CAREFUL!! Pose from KDL is ax ay az aw
# Pose from IKfast is aw ax ay az
n_rot = np.matmul(c_rot, extra_rot)
if inverse:
n_trans = np.matmul(n_rot, extra_translation) + c_trans
else:
n_trans = np.matmul(c_rot, extra_translation) + c_trans
if rot_type == 'matrix':
return n_trans.flatten(), n_rot
quat_rot = np.roll(pyquaternion.Quaternion(matrix=n_rot).normalised.elements, -1)
if rot_type == 'euler':
euler = np.array(tr.euler_from_quaternion(quat_rot, axes='rxyz'))
return np.concatenate((n_trans.flatten(), euler))
elif rot_type == 'quaternion':
return np.concatenate((n_trans.flatten(), quat_rot))
def inverse_transformation(pose, transform):
inv_ee_transform = np.copy(transform)
inv_ee_transform[:3] *= -1
inv_ee_transform[3:] = tr.quaternion_inverse(transform[3:])
return np.array(transform_end_effector(pose, inv_ee_transform, inverse=True))
def to_float(val):
if isinstance(val, float):
return val
elif isinstance(val, str):
return (float(eval(val)))
elif isinstance(val, list):
return [to_float(o) for o in val]
else:
return (float(val))
def to_pose_stamped(frame_id, pose):
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = to_pose(pose)
return ps
def transform_pose(target_frame, transform_matrix, ps):
# def transformPose(self, target_frame, ps):
"""
:param target_frame: the tf target frame, a string
:param ps: the geometry_msgs.msg.PoseStamped message
:return: new geometry_msgs.msg.PoseStamped message, in frame target_frame
:raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise
Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message.
"""
# mat44 is frame-to-frame transform as a 4x4
mat44 = transform_matrix
# pose44 is the given pose as a 4x4
pose44 = np.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation))
# txpose is the new pose in target_frame as a 4x4
txpose = np.dot(mat44, pose44)
# xyz and quat are txpose's position and orientation
xyz = tuple(tr.translation_from_matrix(txpose))[:3]
quat = tuple(tr.rotation_matrix_from_quaternion(txpose))
# assemble return value PoseStamped
r = PoseStamped()
r.header.stamp = ps.header.stamp
r.header.frame_id = target_frame
r.pose = Pose(Point(*xyz), Quaternion(*quat))
return r
def xyz_to_mat44(pos):
return tr.translation_matrix((pos.x, pos.y, pos.z))
def xyzw_to_mat44(ori):
return tr.rotation_matrix_from_quaternion((ori.x, ori.y, ori.z, ori.w))
================================================
FILE: ur_control/src/ur_control/exceptions.py
================================================
class InverseKinematicsException(Exception):
"IK solver failed to find a solution"
pass
================================================
FILE: ur_control/src/ur_control/filters.py
================================================
# ROS utilities used by the CRI group
#! /usr/bin/env python
import math
import numpy as np
import scipy.signal
def best_fit_foaw(y, fs, m, d):
"""
First-Order Adaptive Windowing (FOAW)
Yet another differentiation filter.
@type y: list
@param y: the values of the time history of the signal.
@type fs: float
@param fs: The sampling frequency (Hz) of the signal to be filtered
"""
T = 1.0/fs
result = np.zeros(len(y))
for k in range(len(y)):
slope = 0
for n in range(1, min(m,k)):
# Calculate slope over interval (best-fit-FOAW)
b = ( ( n*sum([y[k-i] for i in range(n+1)])
- 2*sum([y[k-i]*i for i in range(n+1)]) )
/ (T*n*(n+1)*(n+2)/6) )
# Check the linear estimate of each middle point
outside = False
for j in range(1,n):
ykj = y[k]-(b*j*T)
# Compare to the measured value within the noise margin
# If it's outside noise margin, return last estimate
#~ if abs(y[k-j] - ykj) > 2*d:
if ykj < (y[k-j]-d) or ykj > (y[k-j]+d):
outside = True
break
if outside: break
slope = b
result[k] = slope
return result
def butter_lowpass(cutoff, fs, order=5):
"""
Butterworth lowpass digital filter design.
Check C{scipy.signal.butter} for further details.
@type cutoff: float
@param cutoff: Cut-off frequency in Hz
@type fs: float
@param fs: The sampling frequency (Hz) of the signal to be filtered
@type order: int
@param order: The order of the filter.
@rtype: b, a
@return: Numerator (b) and denominator (a) polynomials of the IIR
filter.
"""
nyq = 0.5 * fs
normal_cutoff = cutoff / nyq
b, a = scipy.signal.butter(order, normal_cutoff, btype='low', analog=False)
return b, a
def lowpass_fo(cutoff, fs):
"""
First-order Lowpass digital filter design.
@type cutoff: float
@param cutoff: Cut-off frequency in Hz
@type fs: float
@param fs: The sampling frequency (Hz) of the signal to be filtered
@rtype: b, a
@return: Numerator (b) and denominator (a) polynomials of the IIR
filter.
"""
import control.matlab
F = control.matlab.tf(1,np.array([1/(2*np.pi*cutoff),1]))
Fz = control.matlab.c2d(F, 1/fs, 'zoh')
b = Fz.num[0][0][-1]
a = Fz.den[0][0]
return b, a
def savitzky_golay(y, window_size, order, deriv=0, rate=1):
"""
Smooth (and optionally differentiate) data with a Savitzky-Golay filter.
The Savitzky-Golay filter removes high frequency noise from data.
It has the advantage of preserving the original shape and
features of the signal better than other types of filtering
approaches, such as moving averages techniques.
Notes
=====
The Savitzky-Golay is a type of low-pass filter, particularly suited
for smoothing noisy data. The main idea behind this approach is to
make for each point a least-square fit with a polynomial of high order
over a odd-sized window centered at the point.
Example
=======
>>> import criros
>>> import numpy as np
>>> import matplotlib.pyplot as plt
>>> t = np.linspace(-4, 4, 500)
>>> y = np.exp( -t**2 ) + np.random.normal(0, 0.05, t.shape)
>>> ysg = criros.filters.savitzky_golay(y, window_size=31, order=4)
>>> plt.plot(t, y, label='Noisy signal')
>>> plt.plot(t, np.exp(-t**2), 'k', lw=1.5, label='Original signal')
>>> plt.plot(t, ysg, 'r', label='Filtered signal')
>>> plt.legend()
>>> plt.grid(True)
>>> plt.show()
@type y: array_like, shape (N,)
@param y: the values of the time history of the signal.
@type window_size: int
@param window_size: the length of the window. Must be an odd integer number.
@type order: int
@param order: the order of the polynomial used in the filtering. Must be less than C{window_size} - 1.
@type deriv: int
@param deriv: the order of the derivative to compute (default = 0 means only smoothing)
@rtype: ndarray, shape (N)
@return: the smoothed signal (or it's n-th derivative).
"""
import numpy as np
from math import factorial
try:
window_size = np.abs(np.int(window_size))
order = np.abs(np.int(order))
except ValueError as msg:
raise ValueError("window_size and order have to be of type int")
if window_size % 2 != 1 or window_size < 1:
raise TypeError("window_size size must be a positive odd number")
if window_size < order + 2:
raise TypeError("window_size is too small for the polynomials order")
order_range = list(range(order+1))
half_window = (window_size -1) // 2
# precompute coefficients
b = np.mat([[k**i for i in order_range] for k in range(-half_window, half_window+1)])
m = np.linalg.pinv(b).A[deriv] * rate**deriv * factorial(deriv)
# pad the signal at the extremes with
# values taken from the signal itself
firstvals = y[0] - np.abs( y[1:half_window+1][::-1] - y[0] )
lastvals = y[-1] + np.abs(y[-half_window-1:-1][::-1] - y[-1])
y = np.concatenate((firstvals, y, lastvals))
return np.convolve( m[::-1], y, mode='valid')
def smooth_diff(n):
"""
A smoothed differentiation filter (digital differentiator).
Such a filter has the following advantages:
First, the filter involves both the smoothing operation and
differentiation operation. It can be regarded as a low-pass
differentiation filter (digital differentiator).
It is well known that the common differentiation operation amplifies
the high-frequency noises. Therefore, the smoothed differentiation
filter would be valuable in experimental (noisy) data processing.
Secondly, the filter coefficients are all convenient integers (simple
units) except for an integer scaling factor, as may be especially
significant in some applications such as those in some single-chip
microcomputers or digital signal processors.
Example
=======
>>> import math
>>> import criros
>>> import scipy.signal
>>> import numpy as np
>>> import matplotlib.pyplot as plt
>>> fs = 1. # Hz
>>> samples = 250*fs
>>> t = np.linspace(0, 3. / fs, num=samples)
>>> noise = np.random.normal(0, 0.05, t.shape)
>>> pos = np.sin(2*np.pi*fs*t)
>>> vel = np.diff(pos+noise) * fs
>>> smooth_window = 21
>>> b = -criros.filters.smooth_diff(smooth_window)
>>> vel_smooth = scipy.signal.lfilter(b, 1., pos+noise) * fs
>>> plt.figure(), plt.subplot(211), plt.grid(True)
>>> plt.plot(t, pos, 'k', lw=2.0, label='Original signal')
>>> plt.plot(t, pos+noise, 'r', label='Noisy signal')
>>> plt.legend(loc='best')
>>> plt.ylabel(r'Position')
>>> plt.subplot(212), plt.grid(True)
>>> plt.plot(t[1:], vel, label='np.diff')
>>> left_off = int(math.ceil(smooth_window / 2.0))
>>> right_off = int(smooth_window - left_off)
>>> plt.plot(t[left_off:-right_off], vel_smooth[smooth_window:], lw=2.0, label='smooth_diff')
>>> plt.legend(loc='best')
>>> plt.ylabel(r'Velocity')
>>> plt.show()
References
==========
1. Usui, S.; Amidror, I., I{Digital Low-Pass Differentiation for
Biological Signal-Processing}. IEEE Transactions on Biomedical
Engineering 1982, 29, (10), 686-693.
2. Luo, J. W.; Bai, J.; He, P.; Ying, K., I{Axial strain calculation
using a low-pass digital differentiator in ultrasound
elastography}. IEEE Transactions on Ultrasonics Ferroelectrics
and Frequency Control 2004, 51, (9), 1119-1127.
@type n: int
@param n: filter length (positive integer larger no less than 2)
@rtype: ndarray, shape (n)
@return: filter coefficients (anti-symmetry)
"""
if n >= 2 and math.floor(n) == math.ceil(n):
if n % 2 == 1: # is odd
m = math.trunc((n-1) / 2.0);
h = np.concatenate( (-np.ones(m), [0], np.ones(m)) ) / (m * (m+1))
else: # is even
m = math.trunc(n / 2.0);
h = np.concatenate( (-np.ones(m), np.ones(m)) ) /m**2
else:
raise TypeError("The input parameter (n) should be a positive integer larger no less than 2.")
return h
class ButterLowPass:
"""
Butterworth lowpass digital filter design.
Check C{scipy.signal.butter} for further details.
"""
def __init__( self, cutoff, fs, order=5 ):
"""
C{ButterLowPass} constructor
@type cutoff: float
@param cutoff: Cut-off frequency in Hz
@type fs: float
@param fs: The sampling frequency (Hz) of the signal to be filtered
@type order: int
@param order: The order of the filter.
"""
nyq = 0.5 * fs
normal_cutoff = cutoff / nyq
self.b, self.a = scipy.signal.butter(order, normal_cutoff, btype='low', analog=False)
def __call__( self, x ):
"""
Filters the input array across its C{axis=0} (each column is
considered as an independent signal). Uses initial conditions (C{zi})
for the filter delays.
@type x: array
@param x: An N-dimensional input array.
@rtype: array
@return: The output of the digital filter.
"""
if not hasattr(self, 'zi'):
cols = x.shape[1]
zi = scipy.signal.lfiltic( self.b, self.a, [] ).tolist() * cols
self.zi = np.array(scipy.signal.lfiltic( self.b, self.a, [] ).tolist() * cols)
self.zi.shape = (-1, cols)
(filtered, self.zi) = scipy.signal.lfilter(self.b, self.a, x, zi=self.zi, axis=0 )
return filtered
class LowPassFO:
"""
Causal implementation of a 1st-order Lowpass digital filter.
"""
def __init__( self, cutoff, fs):
"""
C{LowPassFO} constructor
@type cutoff: float
@param cutoff: Cut-off frequency in Hz
@type fs: float
@param fs: The sampling frequency (Hz) of the signal to be filtered
@type order: int
@param order: The order of the filter.
"""
self.b, self.a = lowpass_fo(cutoff, fs)
def __call__( self, x ):
"""
Filters the input array across its C{axis=0} (each column is
considered as an independent signal). Uses initial conditions (C{zi})
for the filter delays.
@type x: array
@param x: An N-dimensional input array.
@rtype: array
@return: The output of the digital filter.
"""
if not hasattr(self, 'zi'):
cols = x.shape[1]
zi = scipy.signal.lfiltic( self.b, self.a, [] ).tolist() * cols
self.zi = np.array(scipy.signal.lfiltic( self.b, self.a, [] ).tolist() * cols)
self.zi.shape = (-1, cols)
(filtered, self.zi) = scipy.signal.lfilter(self.b, self.a, x, zi=self.zi, axis=0 )
return filtered
================================================
FILE: ur_control/src/ur_control/fzi_cartesian_compliance_controller.py
================================================
# The MIT License (MIT)
#
# Copyright (c) 2023 Cristian Beltran
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# Author: Cristian Beltran
import collections
import threading
import types
import rospy
import numpy as np
from ur_control.arm import Arm
from ur_control import conversions
from ur_control.constants import JOINT_TRAJECTORY_CONTROLLER, CARTESIAN_COMPLIANCE_CONTROLLER, ExecutionResult
from geometry_msgs.msg import WrenchStamped, PoseStamped
import dynamic_reconfigure.client
def is_more_extreme(value, target):
if (np.all(value > 0) and np.all(target > 0)):
return np.all(value > target)
elif (np.all(value < 0) and np.all(target < 0)):
return np.all(value < target)
return False
def convert_selection_matrix_to_parameters(selection_matrix):
return {
"stiffness":
{
"sel_x": selection_matrix[0],
"sel_y": selection_matrix[1],
"sel_z": selection_matrix[2],
"sel_ax": selection_matrix[3],
"sel_ay": selection_matrix[4],
"sel_az": selection_matrix[5],
}
}
def convert_stiffness_to_parameters(stiffness):
return {
"stiffness":
{
"trans_x": stiffness[0],
"trans_y": stiffness[1],
"trans_z": stiffness[2],
"rot_x": stiffness[3],
"rot_y": stiffness[4],
"rot_z": stiffness[5],
}
}
def convert_pd_gains_to_parameters(p_gains, d_gains=[0, 0, 0, 0, 0, 0]):
return {
"trans_x": {"p": p_gains[0], "d": d_gains[0]},
"trans_y": {"p": p_gains[1], "d": d_gains[1]},
"trans_z": {"p": p_gains[2], "d": d_gains[2]},
"rot_x": {"p": p_gains[3], "d": d_gains[3]},
"rot_y": {"p": p_gains[4], "d": d_gains[4]},
"rot_z": {"p": p_gains[5], "d": d_gains[5]}
}
def switch_cartesian_contro
gitextract_k89eecrd/
├── .gitignore
├── .travis.yml
├── .vscode/
│ └── settings.json
├── BUILD-DOCKER-IMAGE.sh
├── LICENSE
├── README.md
├── RUN-DOCKER.sh
├── dependencies.rosinstall
├── docker/
│ ├── Dockerfile
│ ├── bashrc
│ ├── build_container.sh
│ ├── docker-compose.yml
│ └── launch_container.sh
├── ur.code-workspace
├── ur_control/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── gripper_controller.yaml
│ │ ├── ur_controllers.yaml
│ │ └── ur_e_controllers.yaml
│ ├── launch/
│ │ ├── ur_controllers.launch
│ │ └── ur_e_controllers.launch
│ ├── package.xml
│ ├── scripts/
│ │ ├── cartesian_compliance_controller_examples.py
│ │ ├── compliance_controller_examples.py
│ │ ├── controller_examples.py
│ │ ├── ft_filter.py
│ │ ├── getch.py
│ │ ├── imu.py
│ │ ├── joint_position_keyboard.py
│ │ ├── joint_position_mouse6d.py
│ │ ├── moveit_tutorial.py
│ │ └── wrench_republish.py
│ ├── setup.py
│ ├── src/
│ │ └── ur_control/
│ │ ├── __init__.py
│ │ ├── arm.py
│ │ ├── compliance_controller.py
│ │ ├── constants.py
│ │ ├── controllers.py
│ │ ├── controllers_connection.py
│ │ ├── conversions.py
│ │ ├── exceptions.py
│ │ ├── filters.py
│ │ ├── fzi_cartesian_compliance_controller.py
│ │ ├── grippers.py
│ │ ├── hybrid_controller.py
│ │ ├── impedance_control.py
│ │ ├── math_utils.py
│ │ ├── mouse_6d.py
│ │ ├── spalg.py
│ │ ├── traj_utils.py
│ │ ├── transformations.py
│ │ ├── ur_services.py
│ │ └── utils.py
│ └── test/
│ ├── test_quaternion_functions.py
│ └── transformations_bk.py
├── ur_gripper_85_moveit_config/
│ ├── .setup_assistant
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── cartesian_limits.yaml
│ │ ├── chomp_planning.yaml
│ │ ├── fake_controllers.yaml
│ │ ├── joint_limits.yaml
│ │ ├── kinematics.yaml
│ │ ├── ompl_planning.yaml
│ │ ├── ros_controllers.yaml
│ │ ├── sensors_3d.yaml
│ │ └── ur_robot_gazebo.srdf
│ ├── launch/
│ │ ├── chomp_planning_pipeline.launch.xml
│ │ ├── default_warehouse_db.launch
│ │ ├── demo.launch
│ │ ├── demo_gazebo.launch
│ │ ├── fake_moveit_controller_manager.launch.xml
│ │ ├── gazebo.launch
│ │ ├── gazebo_static_tf.launch
│ │ ├── joystick_control.launch
│ │ ├── move_group.launch
│ │ ├── moveit.rviz
│ │ ├── moveit_rviz.launch
│ │ ├── ompl_planning_pipeline.launch.xml
│ │ ├── pilz_industrial_motion_planner_planning_pipeline.launch.xml
│ │ ├── planning_context.launch
│ │ ├── planning_pipeline.launch.xml
│ │ ├── ros_controllers.launch
│ │ ├── run_benchmark_ompl.launch
│ │ ├── sensor_manager.launch.xml
│ │ ├── setup_assistant.launch
│ │ ├── start_moveit.launch
│ │ ├── trajectory_execution.launch.xml
│ │ ├── ur_robot_gazebo_moveit_controller_manager.launch.xml
│ │ ├── ur_robot_gazebo_moveit_sensor_manager.launch.xml
│ │ ├── warehouse.launch
│ │ └── warehouse_settings.launch.xml
│ └── package.xml
├── ur_gripper_description/
│ ├── CMakeLists.txt
│ ├── config/
│ │ └── config.rviz
│ ├── launch/
│ │ ├── bringup_with_gripper_85.launch
│ │ ├── display_with_gripper_85.launch
│ │ ├── display_with_gripper_hande.launch
│ │ ├── load_ur_gripper_85.launch.xml
│ │ └── load_ur_gripper_hande.launch.xml
│ ├── package.xml
│ └── urdf/
│ ├── ur_gripper_85.xacro
│ └── ur_gripper_hande.xacro
├── ur_gripper_gazebo/
│ ├── CMakeLists.txt
│ ├── launch/
│ │ ├── gazebo_to_tf.launch
│ │ ├── inc/
│ │ │ ├── load_ur_gripper_85.launch.xml
│ │ │ └── load_ur_gripper_hande.launch.xml
│ │ ├── ur3_cubes_example.launch
│ │ ├── ur3e_cubes_example.launch
│ │ ├── ur3e_with_gripper.launch
│ │ ├── ur5_cubes_example.launch
│ │ ├── ur_gripper_85_cubes.launch
│ │ └── ur_gripper_hande_cubes.launch
│ ├── models/
│ │ ├── floor/
│ │ │ ├── materials/
│ │ │ │ └── scripts/
│ │ │ │ └── checkboard.material
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── green_table/
│ │ │ ├── materials/
│ │ │ │ └── scripts/
│ │ │ │ └── repeated.material
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── multi_peg_board/
│ │ │ ├── meshes/
│ │ │ │ └── board.dae
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── peg_board/
│ │ │ ├── meshes/
│ │ │ │ └── board.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── simple_peg_board/
│ │ │ ├── meshes/
│ │ │ │ ├── simple_board.dae
│ │ │ │ └── simple_board.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur3_base/
│ │ │ ├── meshes/
│ │ │ │ ├── dualbase-big.dae
│ │ │ │ ├── dualbase.dae
│ │ │ │ └── ur3_base.stl
│ │ │ ├── model-bunri.sdf
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur3_table/
│ │ │ ├── meshes/
│ │ │ │ └── ur3_table.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur3e_base/
│ │ │ ├── materials/
│ │ │ │ └── scripts/
│ │ │ │ └── repeated.material
│ │ │ ├── meshes/
│ │ │ │ ├── ur3e_base.dae
│ │ │ │ ├── ur3e_base.stl
│ │ │ │ └── ur3e_base_table.stl
│ │ │ ├── model.config
│ │ │ ├── model.sdf
│ │ │ └── model_base.sdf
│ │ ├── ur3e_table/
│ │ │ ├── meshes/
│ │ │ │ └── ur3e_table.stl
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── ur_base/
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── visual_marker/
│ │ │ ├── model.config
│ │ │ └── model.sdf
│ │ ├── wood_cube/
│ │ │ ├── model.config
│ │ │ ├── model.rsdf
│ │ │ └── model.sdf
│ │ └── wooden_peg/
│ │ ├── model.config
│ │ ├── model.rsdf
│ │ └── model.sdf
│ ├── package.xml
│ ├── scripts/
│ │ ├── gazebo_to_tf.py
│ │ ├── spawner.py
│ │ └── world_publisher.py
│ ├── setup.py
│ ├── src/
│ │ └── ur_gazebo/
│ │ ├── __init__.py
│ │ ├── basic_models.py
│ │ ├── gazebo_spawner.py
│ │ └── model.py
│ ├── urdf/
│ │ ├── ur_gripper_85.xacro
│ │ └── ur_gripper_hande.xacro
│ └── worlds/
│ ├── cubes_task.world
│ └── ur3e.world
├── ur_hande_moveit_config/
│ ├── .setup_assistant
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── cartesian_limits.yaml
│ │ ├── chomp_planning.yaml
│ │ ├── fake_controllers.yaml
│ │ ├── joint_limits.yaml
│ │ ├── kinematics.yaml
│ │ ├── ompl_planning.yaml
│ │ ├── ros_controllers.yaml
│ │ ├── sensors_3d.yaml
│ │ ├── ur3e.urdf
│ │ ├── ur3e_hande.urdf
│ │ └── ur_robot_gazebo.srdf
│ ├── launch/
│ │ ├── chomp_planning_pipeline.launch.xml
│ │ ├── default_warehouse_db.launch
│ │ ├── demo.launch
│ │ ├── demo_gazebo.launch
│ │ ├── fake_moveit_controller_manager.launch.xml
│ │ ├── gazebo.launch
│ │ ├── gazebo_static_tf.launch
│ │ ├── joystick_control.launch
│ │ ├── move_group.launch
│ │ ├── moveit.rviz
│ │ ├── moveit_rviz.launch
│ │ ├── ompl_planning_pipeline.launch.xml
│ │ ├── pilz_industrial_motion_planner_planning_pipeline.launch.xml
│ │ ├── planning_context.launch
│ │ ├── planning_pipeline.launch.xml
│ │ ├── ros_controllers.launch
│ │ ├── run_benchmark_ompl.launch
│ │ ├── sensor_manager.launch.xml
│ │ ├── setup_assistant.launch
│ │ ├── start_moveit.launch
│ │ ├── trajectory_execution.launch.xml
│ │ ├── ur_robot_gazebo_moveit_controller_manager.launch.xml
│ │ ├── ur_robot_gazebo_moveit_sensor_manager.launch.xml
│ │ ├── warehouse.launch
│ │ └── warehouse_settings.launch.xml
│ └── package.xml
├── ur_handeye_calibration/
│ ├── CMakeLists.txt
│ ├── config/
│ │ ├── calibration_data_apriltag.npy
│ │ ├── settings.yaml
│ │ └── tags.yaml
│ ├── launch/
│ │ ├── apriltag_detect.launch
│ │ ├── aruco_detect.launch
│ │ ├── camera.launch
│ │ └── ur3e_fixed_handeye_calibration.launch
│ ├── package.xml
│ └── scripts/
│ ├── calibrate.py
│ ├── calibrator.py
│ └── capture_camera_poses.py
└── ur_pykdl/
├── CMakeLists.txt
├── LICENSE
├── package.xml
├── scripts/
│ ├── display_urdf.py
│ └── ur_kinematics.py
├── setup.py
├── src/
│ ├── ur_kdl/
│ │ ├── __init__.py
│ │ ├── kdl_kinematics.py
│ │ └── kdl_parser.py
│ └── ur_pykdl/
│ ├── __init__.py
│ └── ur_pykdl.py
└── urdf/
├── ur3.urdf
└── ur3e.urdf
SYMBOL INDEX (629 symbols across 44 files)
FILE: ur_control/scripts/cartesian_compliance_controller_examples.py
function signal_handler (line 39) | def signal_handler(sig, frame):
function move_joints (line 47) | def move_joints():
function move_cartesian (line 55) | def move_cartesian():
function move_force (line 94) | def move_force():
function slicing (line 130) | def slicing():
function admittance_control (line 151) | def admittance_control():
function free_drive (line 166) | def free_drive():
function test (line 213) | def test():
function enable_compliance_control (line 226) | def enable_compliance_control():
function main (line 248) | def main():
FILE: ur_control/scripts/compliance_controller_examples.py
function signal_handler (line 39) | def signal_handler(sig, frame):
function move_joints (line 47) | def move_joints(wait=True):
function spiral_trajectory (line 58) | def spiral_trajectory():
function circular_trajectory (line 83) | def circular_trajectory():
function execute_trajectory (line 108) | def execute_trajectory(trajectory, duration, use_force_control=False, te...
function init_force_control (line 126) | def init_force_control(selection_matrix, dt=0.02):
function full_force_control (line 145) | def full_force_control(
function force_control (line 182) | def force_control():
function main (line 199) | def main():
FILE: ur_control/scripts/controller_examples.py
function move_joints (line 41) | def move_joints(wait=True):
function follow_trajectory (line 52) | def follow_trajectory():
function move_endeffector (line 68) | def move_endeffector():
function move_gripper (line 80) | def move_gripper():
function grasp_naive (line 99) | def grasp_naive():
function grasp_plugin (line 115) | def grasp_plugin():
function get_random_valid_direction (line 136) | def get_random_valid_direction(plane):
function circular_trajectory (line 147) | def circular_trajectory():
function main (line 183) | def main():
FILE: ur_control/scripts/ft_filter.py
class FTsensor (line 38) | class FTsensor(object):
method __init__ (line 40) | def __init__(self, in_topic, namespace="", out_topic=None,
method add_wrench_observation (line 86) | def add_wrench_observation(self, wrench):
method raw_wrench_cb (line 89) | def raw_wrench_cb(self, msg):
method get_filtered_wrench (line 116) | def get_filtered_wrench(self):
method update_wrench_offset (line 122) | def update_wrench_offset(self):
method set_enable_publish (line 129) | def set_enable_publish(self, enable):
method set_enable_filtering (line 132) | def set_enable_filtering(self, enable):
method _srv_zeroing (line 135) | def _srv_zeroing(self, req):
method _srv_publish (line 139) | def _srv_publish(self, req):
method _srv_filtering (line 143) | def _srv_filtering(self, req):
function main (line 148) | def main():
FILE: ur_control/scripts/getch.py
function getch (line 7) | def getch(timeout=0.01):
FILE: ur_control/scripts/imu.py
class ImuFake (line 36) | class ImuFake(object):
method __init__ (line 38) | def __init__(self, topic, namespace="", frequency=500):
function main (line 66) | def main():
FILE: ur_control/scripts/joint_position_keyboard.py
function map_keyboard (line 46) | def map_keyboard():
function main (line 167) | def main():
FILE: ur_control/scripts/joint_position_mouse6d.py
function e2q (line 53) | def e2q(e):
function print_robot_state (line 57) | def print_robot_state():
function start_control (line 62) | def start_control(motion_type="linear"):
function main (line 89) | def main():
FILE: ur_control/scripts/moveit_tutorial.py
function signal_handler (line 41) | def signal_handler(sig, frame):
function all_close (line 49) | def all_close(goal, actual, tolerance):
class MoveGroupPythonIntefaceTutorial (line 72) | class MoveGroupPythonIntefaceTutorial(object):
method __init__ (line 75) | def __init__(self):
method display_basic_info (line 120) | def display_basic_info(self):
method go_to_joint_state (line 141) | def go_to_joint_state(self):
method go_to_pose_goal (line 176) | def go_to_pose_goal(self):
method plan_cartesian_path (line 209) | def plan_cartesian_path(self, scale=1):
method display_trajectory (line 251) | def display_trajectory(self, plan):
method execute_plan (line 277) | def execute_plan(self, plan):
method wait_for_state_update (line 295) | def wait_for_state_update(self, box_is_known=False, box_is_attached=Fa...
method add_box (line 336) | def add_box(self, timeout=4):
method add_box2 (line 361) | def add_box2(self, timeout=4):
method attach_box (line 388) | def attach_box(self, timeout=4):
method detach_box (line 416) | def detach_box(self, name=None, timeout=4):
method remove_box (line 435) | def remove_box(self, name=None, timeout=4):
function main (line 456) | def main():
FILE: ur_control/scripts/wrench_republish.py
class FTsensor (line 36) | class FTsensor(object):
method __init__ (line 38) | def __init__(self):
method cb_raw (line 59) | def cb_raw(self, msg):
function main (line 66) | def main():
FILE: ur_control/src/ur_control/arm.py
class Arm (line 52) | class Arm(object):
method __init__ (line 55) | def __init__(self,
method __init_controllers__ (line 123) | def __init_controllers__(self, gripper_type, joint_names_prefix=None):
method __init_ik_solver__ (line 144) | def __init_ik_solver__(self, base_link, ee_link):
method __init_ft_sensor__ (line 169) | def __init_ft_sensor__(self):
method __ft_callback__ (line 196) | def __ft_callback__(self, msg):
method inverse_kinematics (line 202) | def inverse_kinematics(self,
method end_effector (line 252) | def end_effector(self,
method joint_angle (line 302) | def joint_angle(self, joint: str) -> float:
method joint_angles (line 309) | def joint_angles(self) -> np.ndarray:
method joint_velocity (line 316) | def joint_velocity(self, joint: str) -> float:
method joint_velocities (line 323) | def joint_velocities(self) -> np.ndarray:
method joint_effort (line 329) | def joint_effort(self, joint: str) -> float:
method joint_efforts (line 336) | def joint_efforts(self) -> np.ndarray:
method get_wrench_history (line 342) | def get_wrench_history(self, hist_size=24, hand_frame_control=False):
method get_wrench (line 357) | def get_wrench(self,
method set_joint_positions (line 400) | def set_joint_positions(self,
method set_joint_trajectory (line 446) | def set_joint_trajectory(self,
method set_target_pose (line 486) | def set_target_pose(self,
method set_pose_trajectory (line 516) | def set_pose_trajectory(self,
method move_relative (line 550) | def move_relative(self,
method zero_ft_sensor (line 583) | def zero_ft_sensor(self):
method set_ft_filtering (line 594) | def set_ft_filtering(self, active=True):
FILE: ur_control/src/ur_control/compliance_controller.py
function getAvg (line 20) | def getAvg(prev_avg, x, n):
class CompliantController (line 26) | class CompliantController(Arm):
method __init__ (line 27) | def __init__(self,
method set_hybrid_control_trajectory (line 40) | def set_hybrid_control_trajectory(self, trajectory, max_force_torque, ...
method _actuate (line 173) | def _actuate(self, pose, dt, q_last, reduced_speed, attempts=5):
method set_hybrid_control (line 198) | def set_hybrid_control(self, model, max_force_torque, timeout=5.0, sto...
FILE: ur_control/src/ur_control/constants.py
class IKSolverType (line 4) | class IKSolverType(Enum):
class GripperType (line 10) | class GripperType(Enum):
class ExecutionResult (line 15) | class ExecutionResult(Enum):
function get_arm_joint_names (line 50) | def get_arm_joint_names(prefix):
FILE: ur_control/src/ur_control/controllers.py
class JointControllerBase (line 16) | class JointControllerBase(object):
method __init__ (line 21) | def __init__(self, namespace, timeout, joint_names=None):
method disconnect (line 62) | def disconnect(self):
method get_joint_efforts (line 68) | def get_joint_efforts(self):
method get_joint_positions (line 76) | def get_joint_positions(self):
method get_joint_positions_hist (line 84) | def get_joint_positions_hist(self):
method get_joint_velocities (line 92) | def get_joint_velocities(self):
method joint_states_cb (line 100) | def joint_states_cb(self, msg):
class JointPositionController (line 127) | class JointPositionController(JointControllerBase):
method __init__ (line 134) | def __init__(self, namespace='', timeout=5.0, joint_names=None):
method set_joint_positions (line 175) | def set_joint_positions(self, jnt_positions):
method valid_jnt_command (line 194) | def valid_jnt_command(self, command):
class JointTrajectoryController (line 205) | class JointTrajectoryController(JointControllerBase):
method __init__ (line 224) | def __init__(self, publisher_name='arm_controller', namespace='', time...
method add_point (line 258) | def add_point(self, target_time, positions, velocities=None, accelerat...
method clear_points (line 286) | def clear_points(self):
method get_num_points (line 292) | def get_num_points(self):
method get_result (line 300) | def get_result(self) -> FollowJointTrajectoryResult:
method get_state (line 314) | def get_state(self):
method set_trajectory (line 332) | def set_trajectory(self, trajectory):
method start (line 340) | def start(self, delay=0.1, wait=False):
method stop (line 355) | def stop(self):
method wait (line 361) | def wait(self, timeout=15.0):
method start_no_action_server (line 371) | def start_no_action_server(self):
FILE: ur_control/src/ur_control/controllers_connection.py
class ControllersConnection (line 9) | class ControllersConnection():
method __init__ (line 10) | def __init__(self, namespace=None):
method get_loaded_controllers (line 30) | def get_loaded_controllers(self):
method get_controller_state (line 39) | def get_controller_state(self, controller_name):
method load_controllers (line 50) | def load_controllers(self, controllers_list):
method unload_controllers (line 60) | def unload_controllers(self, controllers_list):
method check_controllers_state (line 71) | def check_controllers_state(self, on_controllers, off_controllers):
method switch_controllers (line 81) | def switch_controllers(self, controllers_on, controllers_off,
method reset_controllers (line 138) | def reset_controllers(self):
method update_controllers_list (line 166) | def update_controllers_list(self, new_controllers_list):
FILE: ur_control/src/ur_control/conversions.py
function from_dict (line 17) | def from_dict(transform_dict):
function from_kdl_vector (line 32) | def from_kdl_vector(vector):
function from_kdl_twist (line 43) | def from_kdl_twist(twist):
function from_point (line 58) | def from_point(msg):
function from_pose (line 69) | def from_pose(msg):
function from_pose_to_list (line 82) | def from_pose_to_list(msg):
function from_quaternion (line 93) | def from_quaternion(msg):
function from_roi (line 104) | def from_roi(msg):
function from_transform (line 110) | def from_transform(msg):
function from_vector3 (line 123) | def from_vector3(msg):
function from_wrench (line 134) | def from_wrench(msg):
function to_quaternion (line 148) | def to_quaternion(array):
function to_point (line 159) | def to_point(array):
function to_pose (line 170) | def to_pose(T):
function to_roi (line 191) | def to_roi(top_left, bottom_right):
function to_transform (line 200) | def to_transform(T):
function to_vector3 (line 218) | def to_vector3(array):
function to_wrench (line 229) | def to_wrench(array):
function from_rviz_vector (line 244) | def from_rviz_vector(value, dtype=float):
function angleAxis_from_euler (line 258) | def angleAxis_from_euler(euler):
function euler_transformation_matrix (line 285) | def euler_transformation_matrix(euler):
function transform_end_effector (line 293) | def transform_end_effector(pose, extra_pose, rot_type='quaternion', inve...
function inverse_transformation (line 327) | def inverse_transformation(pose, transform):
function to_float (line 335) | def to_float(val):
function to_pose_stamped (line 346) | def to_pose_stamped(frame_id, pose):
function transform_pose (line 353) | def transform_pose(target_frame, transform_matrix, ps):
function xyz_to_mat44 (line 384) | def xyz_to_mat44(pos):
function xyzw_to_mat44 (line 388) | def xyzw_to_mat44(ori):
FILE: ur_control/src/ur_control/exceptions.py
class InverseKinematicsException (line 1) | class InverseKinematicsException(Exception):
FILE: ur_control/src/ur_control/filters.py
function best_fit_foaw (line 8) | def best_fit_foaw(y, fs, m, d):
function butter_lowpass (line 43) | def butter_lowpass(cutoff, fs, order=5):
function lowpass_fo (line 64) | def lowpass_fo(cutoff, fs):
function savitzky_golay (line 83) | def savitzky_golay(y, window_size, order, deriv=0, rate=1):
function smooth_diff (line 151) | def smooth_diff(n):
class ButterLowPass (line 226) | class ButterLowPass:
method __init__ (line 232) | def __init__( self, cutoff, fs, order=5 ):
method __call__ (line 247) | def __call__( self, x ):
class LowPassFO (line 267) | class LowPassFO:
method __init__ (line 271) | def __init__( self, cutoff, fs):
method __call__ (line 284) | def __call__( self, x ):
FILE: ur_control/src/ur_control/fzi_cartesian_compliance_controller.py
function is_more_extreme (line 40) | def is_more_extreme(value, target):
function convert_selection_matrix_to_parameters (line 48) | def convert_selection_matrix_to_parameters(selection_matrix):
function convert_stiffness_to_parameters (line 62) | def convert_stiffness_to_parameters(stiffness):
function convert_pd_gains_to_parameters (line 76) | def convert_pd_gains_to_parameters(p_gains, d_gains=[0, 0, 0, 0, 0, 0]):
function switch_cartesian_controllers (line 87) | def switch_cartesian_controllers(func):
class CompliantController (line 108) | class CompliantController(Arm):
method __init__ (line 109) | def __init__(self,
method __del__ (line 162) | def __del__(self):
method target_pose_cb (line 169) | def target_pose_cb(self, data):
method target_wrench_cb (line 172) | def target_wrench_cb(self, data):
method activate_cartesian_controller (line 175) | def activate_cartesian_controller(self):
method activate_joint_trajectory_controller (line 179) | def activate_joint_trajectory_controller(self):
method set_cartesian_target_wrench (line 183) | def set_cartesian_target_wrench(self, wrench: list):
method set_cartesian_target_pose (line 193) | def set_cartesian_target_pose(self, pose: list):
method publish_parameter_update (line 201) | def publish_parameter_update(self, parameters):
method __update_controller_parameter_loop__ (line 209) | def __update_controller_parameter_loop__(self):
method update_controller_parameters (line 227) | def update_controller_parameters(self, parameters: dict):
method update_selection_matrix (line 240) | def update_selection_matrix(self, selection_matrix):
method update_pd_gains (line 244) | def update_pd_gains(self, p_gains, d_gains=[0, 0, 0, 0, 0, 0]):
method update_stiffness (line 248) | def update_stiffness(self, stiffness):
method set_control_mode (line 252) | def set_control_mode(self, mode="parallel"):
method set_position_control_mode (line 262) | def set_position_control_mode(self, enable=True):
method set_hand_frame_control (line 267) | def set_hand_frame_control(self, enable):
method set_end_effector_link (line 271) | def set_end_effector_link(self, end_effector_link):
method set_solver_parameters (line 276) | def set_solver_parameters(self, error_scale=None, iterations=None, pub...
method wait_for_robot_to_stop (line 287) | def wait_for_robot_to_stop(self, wait_time=5):
method execute_compliance_control (line 307) | def execute_compliance_control(self, trajectory: np.array, target_wren...
method sliding_error (line 387) | def sliding_error(self, target_pose, max_scale_error):
FILE: ur_control/src/ur_control/grippers.py
class GripperControllerBase (line 21) | class GripperControllerBase():
method __init__ (line 22) | def __init__(self, namespace='', node_name='', prefix=None, timeout=5....
method open (line 57) | def open(self):
method close (line 60) | def close(self):
method get_position (line 63) | def get_position(self):
method get_velocity (line 66) | def get_velocity(self):
method get_opening_percentage (line 69) | def get_opening_percentage(self):
method joint_states_cb (line 72) | def joint_states_cb(self, msg):
class GripperController (line 99) | class GripperController(GripperControllerBase):
method __init__ (line 100) | def __init__(self, namespace='', prefix=None, timeout=5.0, attach_link...
method close (line 147) | def close(self, wait=True):
method percentage_command (line 150) | def percentage_command(self, value, wait=True):
method command (line 153) | def command(self, value, percentage=False, wait=True):
method _distance_to_angle (line 189) | def _distance_to_angle(self, distance):
method _angle_to_distance (line 194) | def _angle_to_distance(self, angle):
method get_result (line 199) | def get_result(self):
method get_state (line 202) | def get_state(self):
method grab (line 205) | def grab(self, link_name):
method open (line 216) | def open(self, wait=True):
method release (line 219) | def release(self, link_name):
method stop (line 230) | def stop(self):
method wait (line 233) | def wait(self, timeout=15.0):
method get_position (line 236) | def get_position(self):
method get_opening_percentage (line 247) | def get_opening_percentage(self):
class RobotiqGripper (line 251) | class RobotiqGripper(GripperControllerBase):
method __init__ (line 252) | def __init__(self, namespace="", prefix="", timeout=2):
method _gripper_status_callback (line 286) | def _gripper_status_callback(self, msg):
method get_opening_percentage (line 289) | def get_opening_percentage(self):
method close (line 292) | def close(self, force=40.0, velocity=1.0, wait=True):
method open (line 295) | def open(self, velocity=1.0, wait=True, opening_width=None):
method convert_percentage_to_width (line 299) | def convert_percentage_to_width(self, width):
method convert_width_to_percentage (line 307) | def convert_width_to_percentage(self, percentage):
method percentage_command (line 316) | def percentage_command(self, value, wait=True):
method command (line 325) | def command(self, command, force=40.0, velocity=1.0, wait=True):
FILE: ur_control/src/ur_control/hybrid_controller.py
class ForcePositionController (line 14) | class ForcePositionController(object):
method __init__ (line 15) | def __init__(self,
method set_goals (line 36) | def set_goals(self, position=None, force=None):
method reset (line 51) | def reset(self):
method control_position (line 61) | def control_position(self, fc, xv):
method control_position_orientation (line 80) | def control_position_orientation(self, fc, xv):
method control_velocity (line 103) | def control_velocity(self, fc, xv):
FILE: ur_control/src/ur_control/impedance_control.py
class AdmittanceModel (line 15) | class AdmittanceModel():
method __repr__ (line 20) | def __repr__(self):
method __str__ (line 22) | def __str__(self):
method __init__ (line 25) | def __init__(self, inertia, stiffness, damper, dt, method="discretizat...
method reset (line 31) | def reset(self):
method set_constants (line 54) | def set_constants(self, inertia, stiffness, damper, dt, reset=False):
method control (line 77) | def control(self, fc):
method traditional_control (line 86) | def traditional_control(self, fc):
method discretization_control (line 97) | def discretization_control(self, fc):
method integration_control (line 115) | def integration_control(self, fc):
FILE: ur_control/src/ur_control/math_utils.py
function orientation_error_as_rotation_vector (line 5) | def orientation_error_as_rotation_vector(quat_target, quat_source):
function quaternions_orientation_error (line 21) | def quaternions_orientation_error(quat_target, quat_source):
function quaternion_normalize (line 39) | def quaternion_normalize(q):
function quaternion_conjugate (line 53) | def quaternion_conjugate(quaternion):
function quaternion_inverse (line 66) | def quaternion_inverse(quaternion):
function quaternion_multiply (line 79) | def quaternion_multiply(quaternion1, quaternion0):
function quaternion_slerp (line 95) | def quaternion_slerp(quat0, quat1, fraction): # TODO
function quaternion_rotate_vector (line 112) | def quaternion_rotate_vector(quat, vector):
function diff_quaternion (line 127) | def diff_quaternion(quat1, quat2):
function random_quaternion (line 143) | def random_quaternion(rand=None):
function random_rotation_matrix (line 165) | def random_rotation_matrix(rand=None):
function rotation_matrix_from_quaternion (line 180) | def rotation_matrix_from_quaternion(q):
function quaternion_from_matrix (line 195) | def quaternion_from_matrix(matrix):
function quaternion_from_axis_angle (line 209) | def quaternion_from_axis_angle(axis_angle):
function quaternion_from_ortho6 (line 223) | def quaternion_from_ortho6(ortho6):
function axis_angle_from_quaternion (line 237) | def axis_angle_from_quaternion(quat):
function ortho6_from_axis_angle (line 250) | def ortho6_from_axis_angle(axis_angle):
function ortho6_from_quaternion (line 263) | def ortho6_from_quaternion(q):
function rotation_matrix_from_ortho6 (line 277) | def rotation_matrix_from_ortho6(ortho6):
function to_np_quaternion (line 297) | def to_np_quaternion(q: np.ndarray) -> np.quaternion:
function to_np_array (line 310) | def to_np_array(np_q: np.quaternion) -> np.ndarray:
FILE: ur_control/src/ur_control/mouse_6d.py
class Mouse6D (line 5) | class Mouse6D():
method __init__ (line 7) | def __init__(self):
method twist_cb (line 19) | def twist_cb(self, msg):
method joy_cb (line 33) | def joy_cb(self, msg):
function main (line 42) | def main():
FILE: ur_control/src/ur_control/spalg.py
class Plane (line 13) | class Plane(object):
method __init__ (line 16) | def __init__(self, normal=None, point=None, equation=None):
method __repr__ (line 29) | def __repr__(self):
method __str__ (line 36) | def __str__(self):
method coefficients (line 40) | def coefficients(self):
method distance (line 43) | def distance(self, point):
method generate_grid (line 54) | def generate_grid(self, cells=10, side_length=1.0):
method generate_mesh (line 85) | def generate_mesh(self, side_length=1.0, thickness=0.001):
method get_ray_intersection (line 112) | def get_ray_intersection(self, ray_origin, ray_dir, epsilon=1e-6):
method get_transform (line 129) | def get_transform(self):
method project (line 139) | def project(self, point):
function counterclockwise_hull (line 151) | def counterclockwise_hull(hull):
function fit_plane_lstsq (line 166) | def fit_plane_lstsq(XYZ):
function fit_plane_optimize (line 187) | def fit_plane_optimize(points, seed=None):
function fit_plane_solve (line 219) | def fit_plane_solve(XYZ):
function fit_plane_svd (line 241) | def fit_plane_svd(XYZ):
function force_frame_transform (line 262) | def force_frame_transform(bTa):
function inertia_matrix_from_vector (line 278) | def inertia_matrix_from_vector(i):
function L_matrix (line 297) | def L_matrix(w):
function motion_frame_transform (line 312) | def motion_frame_transform(bTa):
function perpendicular_vector (line 333) | def perpendicular_vector(v):
function polygon_area (line 351) | def polygon_area(points, plane=None):
function rotation_matrix_from_axes (line 368) | def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS, point=None):
function skew (line 389) | def skew(v):
function transformation_estimation_svd (line 403) | def transformation_estimation_svd(A, B):
function transformation_between_planes (line 440) | def transformation_between_planes(newplane, oldplane):
function transform_inv (line 461) | def transform_inv(T):
function quaternions_orientation_error (line 480) | def quaternions_orientation_error(Qd, Qc):
function translation_rotation_error (line 502) | def translation_rotation_error(to_pose, from_pose):
function convert_wrench (line 508) | def convert_wrench(wrench_force, pose):
function face_towards (line 518) | def face_towards(target_position, current_pose, up_vector=[0, 0, 1]):
function look_rotation (line 533) | def look_rotation(forward, up=[0, 0, 1]):
function jump_threshold (line 588) | def jump_threshold(trajectory, dt, threshold):
function sensor_torque_to_tcp_force (line 609) | def sensor_torque_to_tcp_force(tcp_position, sensor_torques):
FILE: ur_control/src/ur_control/traj_utils.py
function spiral (line 30) | def spiral(radius, theta_offset, revolutions, steps):
function get_conical_helix_trajectory (line 37) | def get_conical_helix_trajectory(p1, p2, steps, revolutions=5.0):
function get_spiral_trajectory (line 50) | def get_spiral_trajectory(p1, p2, steps, revolutions=5.0, from_center=Fa...
function get_circular_trajectory (line 72) | def get_circular_trajectory(p1, p2, steps, revolutions=1.0, from_center=...
function concat_vec (line 90) | def concat_vec(x, y, z, steps):
function get_plane_direction (line 97) | def get_plane_direction(plane, radius):
function compute_rotation_wiggle (line 110) | def compute_rotation_wiggle(initial_orientation, direction, angle, steps...
function compute_trajectory (line 138) | def compute_trajectory(initial_pose, plane, radius, radius_direction, st...
function compute_1d_sinusoidal_trajectory (line 205) | def compute_1d_sinusoidal_trajectory(num_of_points, amplitude=0.01, peri...
function compute_sinusoidal_trajectory (line 211) | def compute_sinusoidal_trajectory(current_pose, dimension: int, num_of_p...
FILE: ur_control/src/ur_control/transformations.py
function identity_matrix (line 180) | def identity_matrix():
function translation_matrix (line 195) | def translation_matrix(direction):
function translation_from_matrix (line 208) | def translation_from_matrix(matrix):
function reflection_matrix (line 220) | def reflection_matrix(point, normal):
function reflection_from_matrix (line 246) | def reflection_from_matrix(matrix):
function rotation_matrix (line 275) | def rotation_matrix(angle, direction, point=None):
function rotation_from_matrix (line 319) | def rotation_from_matrix(matrix):
function scale_matrix (line 359) | def scale_matrix(factor, origin=None, direction=None):
function scale_from_matrix (line 396) | def scale_from_matrix(matrix):
function projection_matrix (line 437) | def projection_matrix(point, normal, direction=None,
function projection_from_matrix (line 499) | def projection_from_matrix(matrix, pseudo=False):
function clip_matrix (line 572) | def clip_matrix(left, right, bottom, top, near, far, perspective=False):
function shear_matrix (line 624) | def shear_matrix(angle, direction, point, normal):
function shear_from_matrix (line 655) | def shear_from_matrix(matrix):
function decompose_matrix (line 700) | def decompose_matrix(matrix):
function compose_matrix (line 785) | def compose_matrix(scale=None, shear=None, angles=None, translate=None,
function orthogonalization_matrix (line 838) | def orthogonalization_matrix(lengths, angles):
function superimposition_matrix (line 866) | def superimposition_matrix(v0, v1, scaling=False, usesvd=True):
function euler_matrix (line 968) | def euler_matrix(ai, aj, ak, axes='sxyz'):
function euler_from_matrix (line 1031) | def euler_from_matrix(matrix, axes='sxyz'):
function euler_from_quaternion (line 1089) | def euler_from_quaternion(quaternion, axes='sxyz'):
function quaternion_from_euler (line 1100) | def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
function pose_from_matrix (line 1157) | def pose_from_matrix(matrix):
class Arcball (line 1168) | class Arcball(object):
method __init__ (line 1192) | def __init__(self, initial=None):
method place (line 1219) | def place(self, center, radius):
method setaxes (line 1232) | def setaxes(self, *axes):
method setconstrain (line 1239) | def setconstrain(self, constrain):
method getconstrain (line 1243) | def getconstrain(self):
method down (line 1247) | def down(self, point):
method drag (line 1258) | def drag(self, point):
method next (line 1274) | def next(self, acceleration=0.0):
method matrix (line 1279) | def matrix(self):
function arcball_map_to_sphere (line 1284) | def arcball_map_to_sphere(point, center, radius):
function arcball_constrain_to_axis (line 1297) | def arcball_constrain_to_axis(point, axis):
function arcball_nearest_axis (line 1313) | def arcball_nearest_axis(point, axes):
function vector_norm (line 1348) | def vector_norm(data, axis=None, out=None):
function unit_vector (line 1387) | def unit_vector(data, axis=None, out=None):
function random_vector (line 1431) | def random_vector(size):
function inverse_matrix (line 1446) | def inverse_matrix(matrix):
function concatenate_matrices (line 1462) | def concatenate_matrices(*matrices):
function is_same_transform (line 1478) | def is_same_transform(matrix0, matrix1):
function rotate_quaternion_by_rpy (line 1494) | def rotate_quaternion_by_rpy(roll, pitch, yaw, q_in, rotated_frame=False):
function translate_pose (line 1510) | def translate_pose(pose, delta, rotated_frame=False):
function transform_pose (line 1528) | def transform_pose(pose, transformation, rotated_frame=False):
function pose_from_angular_velocity_euler (line 1549) | def pose_from_angular_velocity_euler(pose, velocity, dt=1.0):
function pose_from_angular_velocity (line 1576) | def pose_from_angular_velocity(pose, velocity, dt=1.0, rotated_frame=Fal...
function integrateUnitQuaternionDMM (line 1609) | def integrateUnitQuaternionDMM(q, w, dt):
function integrateUnitQuaternionEuler (line 1618) | def integrateUnitQuaternionEuler(q, w, dt):
function pose_to_transform (line 1625) | def pose_to_transform(pose):
function angular_velocity_from_quaternions (line 1635) | def angular_velocity_from_quaternions(q0, q1, dt):
function pose_quaternion_to_euler (line 1639) | def pose_quaternion_to_euler(pose):
function pose_euler_to_quat (line 1643) | def pose_euler_to_quat(pose):
function transform_between_poses (line 1647) | def transform_between_poses(p1, p2):
FILE: ur_control/src/ur_control/ur_services.py
function check_for_real_robot (line 15) | def check_for_real_robot(func):
class URServices (line 26) | class URServices():
method __init__ (line 31) | def __init__(self, namespace):
method safety_mode_callback (line 66) | def safety_mode_callback(self, msg):
method ros_control_status_callback (line 70) | def ros_control_status_callback(self, msg):
method is_running_normally (line 74) | def is_running_normally(self):
method is_protective_stopped (line 81) | def is_protective_stopped(self):
method unlock_protective_stop (line 88) | def unlock_protective_stop(self):
method set_speed_scale (line 110) | def set_speed_scale(self, scale):
method set_payload (line 118) | def set_payload(self, mass, center_of_gravity):
method wait_for_control_status_to_turn_on (line 135) | def wait_for_control_status_to_turn_on(self, waittime):
method activate_ros_control_on_ur (line 146) | def activate_ros_control_on_ur(self, recursion_depth=0):
method check_loaded_program (line 221) | def check_loaded_program(self):
method check_for_dead_controller_and_force_start (line 249) | def check_for_dead_controller_and_force_start(self):
method load_and_execute_program (line 269) | def load_and_execute_program(self, program_name="", recursion_depth=0,...
method load_program (line 277) | def load_program(self, program_name="", recursion_depth=0):
method execute_loaded_program (line 326) | def execute_loaded_program(self):
method close_ur_popup (line 341) | def close_ur_popup(self):
FILE: ur_control/src/ur_control/utils.py
function load_urdf_string (line 16) | def load_urdf_string(package, filename):
class PDRotation (line 25) | class PDRotation:
method __init__ (line 26) | def __init__(self, kp, kd=None):
method reset (line 31) | def reset(self):
method set_gains (line 35) | def set_gains(self, kp=None, kd=None):
method update (line 41) | def update(self, quaternion_error, dt=None):
class PID (line 59) | class PID:
method __init__ (line 60) | def __init__(self, Kp, Ki=None, Kd=None, dynamic_pid=False, max_gain_m...
method reset (line 77) | def reset(self):
method set_gains (line 82) | def set_gains(self, Kp=None, Ki=None, Kd=None):
method set_windup (line 90) | def set_windup(self, windup):
method update (line 94) | def update(self, error, dt=None):
class TextColors (line 137) | class TextColors:
method disable (line 150) | def disable(self):
method blue (line 161) | def blue(self, msg):
method debug (line 169) | def debug(self, msg):
method error (line 177) | def error(self, msg):
method ok (line 185) | def ok(self, msg):
method warning (line 193) | def warning(self, msg):
method logdebug (line 201) | def logdebug(self, msg):
method loginfo (line 211) | def loginfo(self, msg):
method logwarn (line 221) | def logwarn(self, msg):
method logerr (line 231) | def logerr(self, msg):
method logfatal (line 241) | def logfatal(self, msg):
method set_log_level (line 251) | def set_log_level(self, level):
function assert_shape (line 266) | def assert_shape(variable, name, shape):
function assert_type (line 279) | def assert_type(variable, name, ttype):
function db_error_msg (line 292) | def db_error_msg(name, logger=TextColors()):
function clean_cos (line 304) | def clean_cos(value):
function has_keys (line 315) | def has_keys(data, keys):
function raise_not_implemented (line 335) | def raise_not_implemented():
function topic_exist (line 342) | def topic_exist(topic):
function read_key (line 349) | def read_key(echo=False):
function resolve_parameter (line 364) | def resolve_parameter(value, default_value):
function read_parameter (line 370) | def read_parameter(name, default):
function read_parameter_err (line 392) | def read_parameter_err(name):
function read_parameter_fatal (line 414) | def read_parameter_fatal(name):
function solve_namespace (line 434) | def solve_namespace(namespace=None):
function sorted_joint_state_msg (line 455) | def sorted_joint_state_msg(msg, joint_names):
function unique (line 486) | def unique(data):
function wait_for (line 503) | def wait_for(predicate, timeout=5.0):
FILE: ur_control/test/test_quaternion_functions.py
class TestQuaternionFunctions (line 27) | class TestQuaternionFunctions(unittest.TestCase):
method setUp (line 28) | def setUp(self):
method test_orientation_error_as_rotation_vector (line 34) | def test_orientation_error_as_rotation_vector(self):
method test_quaternions_orientation_error (line 39) | def test_quaternions_orientation_error(self):
method test_quaternion_normalize (line 44) | def test_quaternion_normalize(self):
method test_quaternion_conjugate (line 51) | def test_quaternion_conjugate(self):
method test_quaternion_inverse (line 57) | def test_quaternion_inverse(self):
method test_quaternion_multiply (line 63) | def test_quaternion_multiply(self):
method test_quaternion_slerp (line 70) | def test_quaternion_slerp(self):
method test_quaternion_rotate_vector (line 76) | def test_quaternion_rotate_vector(self):
method test_diff_quaternion (line 84) | def test_diff_quaternion(self):
method test_random_quaternion (line 91) | def test_random_quaternion(self):
method test_random_rotation_matrix (line 96) | def test_random_rotation_matrix(self):
method test_rotation_matrix_from_quaternion (line 101) | def test_rotation_matrix_from_quaternion(self):
method test_quaternion_from_axis_angle (line 109) | def test_quaternion_from_axis_angle(self):
method test_quaternion_from_ortho6 (line 116) | def test_quaternion_from_ortho6(self):
method test_axis_angle_from_quaternion (line 124) | def test_axis_angle_from_quaternion(self):
method test_ortho6_from_axis_angle (line 131) | def test_ortho6_from_axis_angle(self):
method test_ortho6_from_quaternion (line 138) | def test_ortho6_from_quaternion(self):
method test_rotation_matrix_from_ortho6 (line 145) | def test_rotation_matrix_from_ortho6(self):
FILE: ur_control/test/transformations_bk.py
function identity_matrix (line 179) | def identity_matrix():
function translation_matrix (line 194) | def translation_matrix(direction):
function translation_from_matrix (line 207) | def translation_from_matrix(matrix):
function reflection_matrix (line 219) | def reflection_matrix(point, normal):
function reflection_from_matrix (line 245) | def reflection_from_matrix(matrix):
function rotation_matrix (line 274) | def rotation_matrix(angle, direction, point=None):
function rotation_from_matrix (line 318) | def rotation_from_matrix(matrix):
function scale_matrix (line 358) | def scale_matrix(factor, origin=None, direction=None):
function scale_from_matrix (line 395) | def scale_from_matrix(matrix):
function projection_matrix (line 436) | def projection_matrix(point, normal, direction=None,
function projection_from_matrix (line 498) | def projection_from_matrix(matrix, pseudo=False):
function clip_matrix (line 571) | def clip_matrix(left, right, bottom, top, near, far, perspective=False):
function shear_matrix (line 623) | def shear_matrix(angle, direction, point, normal):
function shear_from_matrix (line 654) | def shear_from_matrix(matrix):
function decompose_matrix (line 699) | def decompose_matrix(matrix):
function compose_matrix (line 784) | def compose_matrix(scale=None, shear=None, angles=None, translate=None,
function orthogonalization_matrix (line 837) | def orthogonalization_matrix(lengths, angles):
function superimposition_matrix (line 865) | def superimposition_matrix(v0, v1, scaling=False, usesvd=True):
function euler_matrix (line 967) | def euler_matrix(ai, aj, ak, axes='sxyz'):
function euler_from_matrix (line 1030) | def euler_from_matrix(matrix, axes='sxyz'):
function euler_from_quaternion (line 1088) | def euler_from_quaternion(quaternion, axes='sxyz'):
function quaternion_normalize (line 1099) | def quaternion_normalize(v, tolerance=0.00001):
function quaternion_from_euler (line 1107) | def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
function quaternion_about_axis (line 1164) | def quaternion_about_axis(angle, axis):
function quaternion_matrix (line 1181) | def quaternion_matrix(quaternion):
function quaternion_from_matrix (line 1203) | def quaternion_from_matrix(matrix):
function pose_quaternion_from_matrix (line 1235) | def pose_quaternion_from_matrix(matrix):
function quaternion_multiply (line 1246) | def quaternion_multiply(quaternion1, quaternion0):
function quaternion_conjugate (line 1263) | def quaternion_conjugate(quaternion):
function quaternion_inverse (line 1276) | def quaternion_inverse(quaternion):
function quaternion_slerp (line 1288) | def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True):
function quaternion_rotate_vector (line 1329) | def quaternion_rotate_vector(quaternion, vector):
function random_quaternion (line 1337) | def random_quaternion(rand=None):
function random_rotation_matrix (line 1367) | def random_rotation_matrix(rand=None):
class Arcball (line 1382) | class Arcball(object):
method __init__ (line 1406) | def __init__(self, initial=None):
method place (line 1433) | def place(self, center, radius):
method setaxes (line 1446) | def setaxes(self, *axes):
method setconstrain (line 1453) | def setconstrain(self, constrain):
method getconstrain (line 1457) | def getconstrain(self):
method down (line 1461) | def down(self, point):
method drag (line 1472) | def drag(self, point):
method next (line 1488) | def next(self, acceleration=0.0):
method matrix (line 1493) | def matrix(self):
function arcball_map_to_sphere (line 1498) | def arcball_map_to_sphere(point, center, radius):
function arcball_constrain_to_axis (line 1511) | def arcball_constrain_to_axis(point, axis):
function arcball_nearest_axis (line 1527) | def arcball_nearest_axis(point, axes):
function vector_norm (line 1562) | def vector_norm(data, axis=None, out=None):
function unit_vector (line 1601) | def unit_vector(data, axis=None, out=None):
function random_vector (line 1645) | def random_vector(size):
function inverse_matrix (line 1660) | def inverse_matrix(matrix):
function concatenate_matrices (line 1676) | def concatenate_matrices(*matrices):
function is_same_transform (line 1692) | def is_same_transform(matrix0, matrix1):
function _import_module (line 1708) | def _import_module(module_name, warn=True, prefix='_py_', ignore='_'):
function rotate_quaternion_by_rpy (line 1735) | def rotate_quaternion_by_rpy(roll, pitch, yaw, q_in, rotated_frame=False):
function translate_pose (line 1751) | def translate_pose(pose, delta, rotated_frame=False):
function transform_pose (line 1769) | def transform_pose(pose, transformation, rotated_frame=False):
function pose_from_angular_velocity_euler (line 1790) | def pose_from_angular_velocity_euler(pose, velocity, dt=1.0):
function pose_from_angular_velocity (line 1817) | def pose_from_angular_velocity(pose, velocity, dt=1.0, rotated_frame=Fal...
function integrateUnitQuaternionDMM (line 1850) | def integrateUnitQuaternionDMM(q, w, dt):
function integrateUnitQuaternionDMM2 (line 1859) | def integrateUnitQuaternionDMM2(q, w, dt):
function integrateUnitQuaternionEuler (line 1869) | def integrateUnitQuaternionEuler(q, w, dt):
function pose_to_transform2 (line 1876) | def pose_to_transform2(pose):
function pose_to_transform (line 1892) | def pose_to_transform(pose):
function angular_velocity_from_quaternions (line 1902) | def angular_velocity_from_quaternions(q0, q1, dt):
function vector_to_pyquaternion (line 1920) | def vector_to_pyquaternion(vector):
function vector_from_pyquaternion (line 1924) | def vector_from_pyquaternion(quat):
function pose_quaternion_to_euler (line 1928) | def pose_quaternion_to_euler(pose):
function pose_euler_to_quat (line 1932) | def pose_euler_to_quat(pose):
function diff_quaternion (line 1936) | def diff_quaternion(q1, q2):
function transform_between_poses (line 1940) | def transform_between_poses(p1, p2):
function quaternion_from_axis_angle (line 1948) | def quaternion_from_axis_angle(axis_angle):
function axis_angle_from_quaternion (line 1962) | def axis_angle_from_quaternion(quat):
function quaternion_from_ortho6 (line 1987) | def quaternion_from_ortho6(ortho6):
function ortho6_from_axis_angle (line 1992) | def ortho6_from_axis_angle(axis_angle):
function ortho6_from_quaternion (line 1996) | def ortho6_from_quaternion(q):
function rotation_matrix_from_ortho6 (line 2001) | def rotation_matrix_from_ortho6(ortho6):
FILE: ur_gripper_gazebo/scripts/gazebo_to_tf.py
class GazeboToTf (line 12) | class GazeboToTf:
method __init__ (line 15) | def __init__(self):
method callback (line 20) | def callback(self, data):
FILE: ur_gripper_gazebo/scripts/spawner.py
function place_target (line 16) | def place_target():
function place_ball (line 26) | def place_ball():
function place_cube (line 35) | def place_cube():
function place_models (line 44) | def place_models():
function place_soft (line 52) | def place_soft():
function place_door (line 69) | def place_door():
function place_aruco (line 76) | def place_aruco():
function main (line 85) | def main():
FILE: ur_gripper_gazebo/scripts/world_publisher.py
function gazebo_callback (line 17) | def gazebo_callback(msg):
FILE: ur_gripper_gazebo/src/ur_gazebo/gazebo_spawner.py
function delete_gazebo_models (line 12) | def delete_gazebo_models(models):
class GazeboModels (line 26) | class GazeboModels:
method __init__ (line 29) | def __init__(self, model_pkg):
method load_models (line 44) | def load_models(self, models):
method _gazebo_callback (line 51) | def _gazebo_callback(self, data):
method reset_models (line 57) | def reset_models(self, models):
method reset_model (line 61) | def reset_model(self, model):
method update_models_state (line 70) | def update_models_state(self, models):
method update_model_state (line 74) | def update_model_state(self, model):
method load_urdf_model (line 84) | def load_urdf_model(self, model):
method load_sdf_model (line 97) | def load_sdf_model(self, model):
method load_xml (line 112) | def load_xml(self, model_name, filetype="sdf"):
FILE: ur_gripper_gazebo/src/ur_gazebo/model.py
class Model (line 9) | class Model(object):
method __init__ (line 11) | def __init__(self, name, position, orientation=[0,0,0,1], file_type='u...
method set_pose (line 29) | def set_pose(self, position, orientation=[0,0,0,1]):
method get_rotation (line 38) | def get_rotation(self):
method get_pose (line 41) | def get_pose(self):
FILE: ur_handeye_calibration/scripts/calibrate.py
function calibrate_simulation (line 23) | def calibrate_simulation(cto_poses, bte_poses):
function main (line 53) | def main():
FILE: ur_handeye_calibration/scripts/calibrator.py
function signal_handler (line 21) | def signal_handler(sig, frame):
function append_tf_data (line 54) | def append_tf_data():
function rotate_wrist (line 70) | def rotate_wrist(q, changes):
function move_arm (line 80) | def move_arm(wait=True):
function main (line 128) | def main():
FILE: ur_handeye_calibration/scripts/capture_camera_poses.py
function signal_handler (line 20) | def signal_handler(sig, frame):
function append_tf_data (line 44) | def append_tf_data():
function rotate_wrist (line 60) | def rotate_wrist(q, changes):
function move_arm (line 70) | def move_arm(wait=True):
function main (line 121) | def main():
FILE: ur_pykdl/scripts/display_urdf.py
function main (line 36) | def main():
FILE: ur_pykdl/scripts/ur_kinematics.py
function main (line 35) | def main():
FILE: ur_pykdl/src/ur_kdl/kdl_kinematics.py
function create_kdl_kin (line 43) | def create_kdl_kin(base_link, end_link, urdf_filename=None):
class KDLKinematics (line 54) | class KDLKinematics(object):
method __init__ (line 62) | def __init__(self, urdf, base_link, end_link, kdl_tree=None):
method get_link_names (line 121) | def get_link_names(self, joints=False, fixed=True):
method get_joint_names (line 126) | def get_joint_names(self, links=False, fixed=False):
method get_joint_limits (line 130) | def get_joint_limits(self):
method FK (line 133) | def FK(self, q, link_number=None):
method forward (line 150) | def forward(self, q, end_link=None, base_link=None):
method _do_kdl_fk (line 178) | def _do_kdl_fk(self, q, link_number):
method inverse (line 203) | def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
method inverse_search (line 239) | def inverse_search(self, pose, timeout=1.):
method jacobian (line 254) | def jacobian(self, q, pos=None):
method inertia (line 269) | def inertia(self, q):
method cart_inertia (line 278) | def cart_inertia(self, q):
method joints_in_limits (line 287) | def joints_in_limits(self, q):
method joints_in_safe_limits (line 296) | def joints_in_safe_limits(self, q):
method clip_joints_safe (line 305) | def clip_joints_safe(self, q):
method random_joint_angles (line 313) | def random_joint_angles(self):
method difference_joints (line 327) | def difference_joints(self, q1, q2):
method inverse_biased (line 342) | def inverse_biased(self, pose, q_init, q_bias, q_bias_weights, rot_wei...
method inverse_biased_search (line 368) | def inverse_biased_search(self, pos, rot, q_bias, q_bias_weights, rot_...
function kdl_to_mat (line 384) | def kdl_to_mat(m):
function joint_kdl_to_list (line 391) | def joint_kdl_to_list(q):
function joint_list_to_kdl (line 396) | def joint_list_to_kdl(q):
function main (line 406) | def main():
FILE: ur_pykdl/src/ur_kdl/kdl_parser.py
function euler_to_quat (line 41) | def euler_to_quat(r, p, y):
function urdf_pose_to_kdl_frame (line 49) | def urdf_pose_to_kdl_frame(pose):
function _toKdlPose (line 60) | def _toKdlPose(pose):
function urdf_joint_to_kdl_joint (line 69) | def urdf_joint_to_kdl_joint(jnt):
function urdf_inertial_to_kdl_rbi (line 87) | def urdf_inertial_to_kdl_rbi(i):
function kdl_tree_from_urdf_model (line 99) | def kdl_tree_from_urdf_model(urdf):
function main (line 123) | def main():
FILE: ur_pykdl/src/ur_pykdl/ur_pykdl.py
function frame_to_list (line 67) | def frame_to_list(frame):
class ur_kinematics (line 75) | class ur_kinematics(object):
method __init__ (line 80) | def __init__(self, base_link=None, ee_link=None, robot=None, prefix=No...
method print_robot_description (line 113) | def print_robot_description(self):
method print_kdl_chain (line 124) | def print_kdl_chain(self):
method joints_to_kdl (line 128) | def joints_to_kdl(self, type, values):
method kdl_to_mat (line 139) | def kdl_to_mat(self, data):
method end_effector_transform (line 146) | def end_effector_transform(self, joint_values, tip_link=None):
method forward (line 153) | def forward(self, joint_values, tip_link=None):
method forward_position_kinematics (line 165) | def forward_position_kinematics(self, joint_values):
method forward_velocity_kinematics (line 171) | def forward_velocity_kinematics(self, joint_velocities):
method inverse_kinematics (line 177) | def inverse_kinematics(self, position, orientation=None, seed=None):
method jacobian (line 206) | def jacobian(self, joint_values=None):
method jacobian_transpose (line 211) | def jacobian_transpose(self, joint_values=None):
method jacobian_pseudo_inverse (line 214) | def jacobian_pseudo_inverse(self, joint_values=None):
method inertia (line 217) | def inertia(self, joint_values=None):
method cart_inertia (line 222) | def cart_inertia(self, joint_values=None):
Condensed preview — 233 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (4,761K chars).
[
{
"path": ".gitignore",
"chars": 473,
"preview": "*.py[cod]\n\n# C extensions\n*.so\n\n# Packages\n*.egg\n*.egg-info\ndist\nbuild\neggs\nparts\nbin\nvar\nsdist\ndevelop-eggs\n.installed."
},
{
"path": ".travis.yml",
"chars": 5008,
"preview": "# Generic .travis.yml file for running continuous integration on Travis-CI for\n# any ROS package.\n#\n# Available here:\n# "
},
{
"path": ".vscode/settings.json",
"chars": 80,
"preview": "{\n \"python.linting.pylintEnabled\": true,\n \"python.linting.enabled\": true\n}"
},
{
"path": "BUILD-DOCKER-IMAGE.sh",
"chars": 1563,
"preview": "#!/bin/bash\n# Generates a docker image with the relevant settings for the DOCKER_PROJECT.\n# Context-sensitive behaviour:"
},
{
"path": "LICENSE",
"chars": 1088,
"preview": "The MIT License (MIT)\n\nCopyright (c) 2018-2023 Cristian Beltran\n\nPermission is hereby granted, free of charge, to any pe"
},
{
"path": "README.md",
"chars": 3154,
"preview": "Universal Robot UR/URe\n===\n<img src=\"https://github.com/cambel/ur3/blob/noetic-devel/wiki/ur3e.gif?raw=true\" alt=\"UR3e &"
},
{
"path": "RUN-DOCKER.sh",
"chars": 995,
"preview": "#!/bin/bash\n\n################################################################################\nexport DOCKER_RUNTIME=nvid"
},
{
"path": "dependencies.rosinstall",
"chars": 877,
"preview": "- git:\n local-name: robotiq\n uri: https://github.com/cambel/robotiq.git\n version: noetic-devel\n- git: \n loc"
},
{
"path": "docker/Dockerfile",
"chars": 2864,
"preview": "# Docker for ur3 repo\n# ros-noetic-base, gazebo11, python3 libraries \n# Python3 3 version\n\nFROM osrf/ros:noetic-desktop-"
},
{
"path": "docker/bashrc",
"chars": 0,
"preview": ""
},
{
"path": "docker/build_container.sh",
"chars": 1550,
"preview": "#!/bin/bash\n# Generates a docker image with the relevant settings for the DOCKER_PROJECT.\n# Context-sensitive behaviour:"
},
{
"path": "docker/docker-compose.yml",
"chars": 1237,
"preview": "version: \"2.4\"\n\n################################################################################\n\nservices:\n ros_ur:\n "
},
{
"path": "docker/launch_container.sh",
"chars": 959,
"preview": "#!/bin/bash\n\n################################################################################\n\n# Set the Docker containe"
},
{
"path": "ur.code-workspace",
"chars": 823,
"preview": "{\n\t\"folders\": [\n\t\t{\n\t\t\t\"path\": \".\"\n\t\t},\n\t],\n\t\"settings\": {\n\t\t\"files.insertFinalNewline\": true,\n\t\t\"files.exclude\": {\n\t\t\t\""
},
{
"path": "ur_control/CMakeLists.txt",
"chars": 427,
"preview": "cmake_minimum_required(VERSION 3.1.3)\nproject(ur_control)\n\nfind_package(catkin REQUIRED COMPONENTS\n control_msgs\n cont"
},
{
"path": "ur_control/config/gripper_controller.yaml",
"chars": 275,
"preview": "# Gripper controller\ngripper_controller:\n type: position_controllers/GripperActionController\n gripper_type: '85'\n "
},
{
"path": "ur_control/config/ur_controllers.yaml",
"chars": 1398,
"preview": "# Joint state controller\njoint_state_controller:\n publish_rate: 125\n type: joint_state_controller/JointStateController"
},
{
"path": "ur_control/config/ur_e_controllers.yaml",
"chars": 1398,
"preview": "# Joint state controller\njoint_state_controller:\n publish_rate: 500\n type: joint_state_controller/JointStateController"
},
{
"path": "ur_control/launch/ur_controllers.launch",
"chars": 1423,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- Launch file parameters -->\n <arg name=\"debug\" default=\"true\" />\n\n <arg if= "
},
{
"path": "ur_control/launch/ur_e_controllers.launch",
"chars": 1493,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- Launch file parameters -->\n <arg name=\"debug\" default=\"true\" />\n"
},
{
"path": "ur_control/package.xml",
"chars": 1494,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>ur_control</name>\n <version>0.1.0</version>\n <description>The ur_control packa"
},
{
"path": "ur_control/scripts/cartesian_compliance_controller_examples.py",
"chars": 11404,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2023 Cristian Beltran\n#\n# Permission is hereby granted,"
},
{
"path": "ur_control/scripts/compliance_controller_examples.py",
"chars": 9022,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/scripts/controller_examples.py",
"chars": 8720,
"preview": "#! /usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gr"
},
{
"path": "ur_control/scripts/ft_filter.py",
"chars": 6810,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2023 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/scripts/getch.py",
"chars": 840,
"preview": "import sys\nimport termios\nimport tty\nfrom select import select\n\n\ndef getch(timeout=0.01):\n \"\"\"\n Retrieves a charac"
},
{
"path": "ur_control/scripts/imu.py",
"chars": 2574,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/scripts/joint_position_keyboard.py",
"chars": 8179,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/scripts/joint_position_mouse6d.py",
"chars": 3515,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/scripts/moveit_tutorial.py",
"chars": 24901,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/scripts/wrench_republish.py",
"chars": 2504,
"preview": "#!/usr/bin/env python\n\n# The MIT License (MIT)\n#\n# Copyright (c) 2018-2021 Cristian Beltran\n#\n# Permission is hereby gra"
},
{
"path": "ur_control/setup.py",
"chars": 219,
"preview": "#!/usr/bin/env python\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd "
},
{
"path": "ur_control/src/ur_control/__init__.py",
"chars": 1,
"preview": "\n"
},
{
"path": "ur_control/src/ur_control/arm.py",
"chars": 25079,
"preview": "# The MIT License (MIT)\n#\n# Copyright (c) 2018-2023 Cristian Beltran\n#\n# Permission is hereby granted, free of charge, t"
},
{
"path": "ur_control/src/ur_control/compliance_controller.py",
"chars": 11985,
"preview": "# Copyright (c) 2018-2021, Cristian Beltran. All rights reserved.\n#\n# Cristian Beltran and its licensors retain all int"
},
{
"path": "ur_control/src/ur_control/constants.py",
"chars": 1277,
"preview": "from enum import Enum\n\n\nclass IKSolverType(Enum):\n KDL = 'kdl'\n TRAC_IK = 'Trac-IK'\n IKFAST = 'IKFast'\n\n\nclass "
},
{
"path": "ur_control/src/ur_control/controllers.py",
"chars": 17068,
"preview": "#!/usr/bin/env python\nimport actionlib\nimport copy\nimport collections\nimport rospy\nfrom ur_control import utils, constan"
},
{
"path": "ur_control/src/ur_control/controllers_connection.py",
"chars": 6422,
"preview": "#!/usr/bin/env python\n\nimport rospy\nfrom controller_manager_msgs.srv import SwitchController, SwitchControllerRequest, \\"
},
{
"path": "ur_control/src/ur_control/conversions.py",
"chars": 11644,
"preview": "#! /usr/bin/env python\n\nimport numpy as np\n\nfrom ur_control import transformations as tr\n\n# Messages\nfrom geometry_msgs."
},
{
"path": "ur_control/src/ur_control/exceptions.py",
"chars": 96,
"preview": "class InverseKinematicsException(Exception):\n \"IK solver failed to find a solution\"\n pass\n"
},
{
"path": "ur_control/src/ur_control/filters.py",
"chars": 10450,
"preview": "# ROS utilities used by the CRI group\n#! /usr/bin/env python\nimport math\nimport numpy as np\nimport scipy.signal\n\n\ndef be"
},
{
"path": "ur_control/src/ur_control/fzi_cartesian_compliance_controller.py",
"chars": 17304,
"preview": "# The MIT License (MIT)\n#\n# Copyright (c) 2023 Cristian Beltran\n#\n# Permission is hereby granted, free of charge, to any"
},
{
"path": "ur_control/src/ur_control/grippers.py",
"chars": 14203,
"preview": "# Gripper action\nimport actionlib\nimport numpy as np\nimport rospy\nfrom sensor_msgs.msg import JointState\nfrom control_ms"
},
{
"path": "ur_control/src/ur_control/hybrid_controller.py",
"chars": 4668,
"preview": "# Copyright (c) 2018-2021, Cristian Beltran. All rights reserved.\n#\n# Cristian Beltran and its licensors retain all int"
},
{
"path": "ur_control/src/ur_control/impedance_control.py",
"chars": 4690,
"preview": "# Copyright (c) 2018-2021, Cristian Beltran. All rights reserved.\n#\n# Cristian Beltran and its licensors retain all int"
},
{
"path": "ur_control/src/ur_control/math_utils.py",
"chars": 8176,
"preview": "import numpy as np\nimport quaternion\n\n\ndef orientation_error_as_rotation_vector(quat_target, quat_source):\n \"\"\"\n C"
},
{
"path": "ur_control/src/ur_control/mouse_6d.py",
"chars": 1536,
"preview": "import rospy\nfrom geometry_msgs.msg import Twist\nfrom sensor_msgs.msg import Joy\n\nclass Mouse6D():\n \"\"\" Subscribe to "
},
{
"path": "ur_control/src/ur_control/spalg.py",
"chars": 20684,
"preview": "#! /usr/bin/env python\nimport rospy\nimport numpy as np\nimport scipy.optimize\nimport ur_control.transformations as tr\nfro"
},
{
"path": "ur_control/src/ur_control/traj_utils.py",
"chars": 9916,
"preview": "# The MIT License (MIT)\n#\n# Copyright (c) 2018, 2019 Cristian Beltran\n#\n# Permission is hereby granted, free of charge, "
},
{
"path": "ur_control/src/ur_control/transformations.py",
"chars": 56793,
"preview": "# -*- coding: utf-8 -*-\n# transformations.py\n\n# Copyright (c) 2006, Christoph Gohlke\n# Copyright (c) 2006-2009, The Rege"
},
{
"path": "ur_control/src/ur_control/ur_services.py",
"chars": 16039,
"preview": "\nimport rospy\nimport time\n\nimport controller_manager_msgs.msg\nimport std_srvs.srv\nfrom ur_control import conversions\nfro"
},
{
"path": "ur_control/src/ur_control/utils.py",
"chars": 15335,
"preview": "# ROS utilities used by the CRI group\n#! /usr/bin/env python\nimport os\nimport sys\nimport copy\nimport time\nimport numpy a"
},
{
"path": "ur_control/test/test_quaternion_functions.py",
"chars": 6852,
"preview": "import numpy as np\nimport unittest\n\nimport transformations_bk as tr\nfrom ur_control.math_utils import (\n orientation_"
},
{
"path": "ur_control/test/transformations_bk.py",
"chars": 67231,
"preview": "# -*- coding: utf-8 -*-\n# transformations.py\n\n# Copyright (c) 2006, Christoph Gohlke\n# Copyright (c) 2006-2009, The Rege"
},
{
"path": "ur_gripper_85_moveit_config/.setup_assistant",
"chars": 782,
"preview": "moveit_setup_assistant_config:\n URDF:\n package: ur_gripper_gazebo\n relative_path: urdf/ur_gripper_85.xacro\n xa"
},
{
"path": "ur_gripper_85_moveit_config/CMakeLists.txt",
"chars": 316,
"preview": "cmake_minimum_required(VERSION 3.1.3)\nproject(ur_gripper_85_moveit_config)\n\nfind_package(catkin REQUIRED)\n\ncatkin_packag"
},
{
"path": "ur_gripper_85_moveit_config/config/cartesian_limits.yaml",
"chars": 99,
"preview": "cartesian_limits:\n max_trans_vel: 1\n max_trans_acc: 2.25\n max_trans_dec: -5\n max_rot_vel: 1.57\n"
},
{
"path": "ur_gripper_85_moveit_config/config/chomp_planning.yaml",
"chars": 486,
"preview": "planning_time_limit: 10.0\nmax_iterations: 200\nmax_iterations_after_collision_free: 5\nsmoothness_cost_weight: 0.1\nobstacl"
},
{
"path": "ur_gripper_85_moveit_config/config/fake_controllers.yaml",
"chars": 408,
"preview": "controller_list:\n - name: fake_arm_controller\n type: $(arg execution_type)\n joints:\n - shoulder_pan_joint\n "
},
{
"path": "ur_gripper_85_moveit_config/config/joint_limits.yaml",
"chars": 2035,
"preview": "# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed\n# Spec"
},
{
"path": "ur_gripper_85_moveit_config/config/kinematics.yaml",
"chars": 155,
"preview": "arm:\n kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin\n kinematics_solver_search_resolution: 0.005"
},
{
"path": "ur_gripper_85_moveit_config/config/ompl_planning.yaml",
"chars": 8785,
"preview": "planner_configs:\n SBL:\n type: geometric::SBL\n range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0"
},
{
"path": "ur_gripper_85_moveit_config/config/ros_controllers.yaml",
"chars": 1166,
"preview": "# Simulation settings for using moveit_sim_controllers\nmoveit_sim_hw_interface:\n joint_model_group: todo_group_name\n j"
},
{
"path": "ur_gripper_85_moveit_config/config/sensors_3d.yaml",
"chars": 105,
"preview": "# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it\nsensors:\n - {}"
},
{
"path": "ur_gripper_85_moveit_config/config/ur_robot_gazebo.srdf",
"chars": 11628,
"preview": "<?xml version=\"1.0\" ?>\n<!--This does not replace URDF, and is not an extension of URDF.\n This is a format for represe"
},
{
"path": "ur_gripper_85_moveit_config/launch/chomp_planning_pipeline.launch.xml",
"chars": 1415,
"preview": "<launch>\n <!-- CHOMP Plugin for MoveIt! -->\n <arg name=\"planning_plugin\" value=\"chomp_interface/CHOMPPlanner\" />\n\n <!"
},
{
"path": "ur_gripper_85_moveit_config/launch/default_warehouse_db.launch",
"chars": 727,
"preview": "<launch>\n\n <arg name=\"reset\" default=\"false\"/>\n <!-- If not specified, we'll use a default database location -->\n <ar"
},
{
"path": "ur_gripper_85_moveit_config/launch/demo.launch",
"chars": 3133,
"preview": "<launch>\n\n <!-- specify the planning pipeline -->\n <arg name=\"pipeline\" default=\"ompl\" />\n\n <!-- By default, we do no"
},
{
"path": "ur_gripper_85_moveit_config/launch/demo_gazebo.launch",
"chars": 3468,
"preview": "<launch>\n\n <!-- By default, we do not start a database (it can be large) -->\n <arg name=\"db\" default=\"false\" />\n <!--"
},
{
"path": "ur_gripper_85_moveit_config/launch/fake_moveit_controller_manager.launch.xml",
"chars": 552,
"preview": "<launch>\n\n <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->\n <arg n"
},
{
"path": "ur_gripper_85_moveit_config/launch/gazebo.launch",
"chars": 990,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"paused\" default=\"false\"/>\n <arg name=\"gazebo_gui\" default=\"true\"/>\n <arg n"
},
{
"path": "ur_gripper_85_moveit_config/launch/gazebo_static_tf.launch",
"chars": 270,
"preview": "<launch>\n <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"floorworld_broadcaster\" args=\"0 0 0 0 0 0 floor worl"
},
{
"path": "ur_gripper_85_moveit_config/launch/joystick_control.launch",
"chars": 621,
"preview": "<launch>\n <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->\n\n <arg name=\"dev\" default=\"/dev/inp"
},
{
"path": "ur_gripper_85_moveit_config/launch/move_group.launch",
"chars": 4277,
"preview": "<launch>\n <!-- Specific ur robot -->\n <arg name=\"ur_robot\" default=\"ur3\"/>\n\n <!-- GDB Debug Option -->\n <arg name=\"d"
},
{
"path": "ur_gripper_85_moveit_config/launch/moveit.rviz",
"chars": 11444,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 84\n Name: Displays\n Property Tree Widget:\n Expanded: ~\n "
},
{
"path": "ur_gripper_85_moveit_config/launch/moveit_rviz.launch",
"chars": 576,
"preview": "<launch>\n\n <arg name=\"debug\" default=\"false\" />\n <arg unless=\"$(arg debug)\" name=\"launch_prefix\" value=\"\" />\n <arg "
},
{
"path": "ur_gripper_85_moveit_config/launch/ompl_planning_pipeline.launch.xml",
"chars": 1319,
"preview": "<launch>\n\n <!-- OMPL Plugin for MoveIt! -->\n <arg name=\"planning_plugin\" value=\"ompl_interface/OMPLPlanner\" />\n\n <!--"
},
{
"path": "ur_gripper_85_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml",
"chars": 1211,
"preview": "<launch>\n\n <!-- Pilz Command Planner Plugin for MoveIt -->\n <arg name=\"planning_plugin\" value=\"pilz_industrial_motion_"
},
{
"path": "ur_gripper_85_moveit_config/launch/planning_context.launch",
"chars": 2799,
"preview": "<launch>\n <arg name=\"ur_robot\" default=\"ur3\"/>\n\n <!-- By default we do not overwrite the URDF. Change the following to"
},
{
"path": "ur_gripper_85_moveit_config/launch/planning_pipeline.launch.xml",
"chars": 695,
"preview": "<launch>\n\n <!-- This file makes it easy to include different planning pipelines;\n It is assumed that all planning"
},
{
"path": "ur_gripper_85_moveit_config/launch/ros_controllers.launch",
"chars": 398,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <!-- Load joint controller configurations from YAML file to parameter server -->\n <ro"
},
{
"path": "ur_gripper_85_moveit_config/launch/run_benchmark_ompl.launch",
"chars": 835,
"preview": "<launch>\n\n <!-- This argument must specify the list of .cfg files to process for benchmarking -->\n <arg name=\"cfg\" />\n"
},
{
"path": "ur_gripper_85_moveit_config/launch/sensor_manager.launch.xml",
"chars": 795,
"preview": "<launch>\n\n <!-- This file makes it easy to include the settings for sensor managers -->\n\n <!-- Params for 3D sensors c"
},
{
"path": "ur_gripper_85_moveit_config/launch/setup_assistant.launch",
"chars": 560,
"preview": "<!-- Re-launch the MoveIt! Setup Assistant with this configuration package already loaded -->\n<launch>\n\n <!-- Debug Inf"
},
{
"path": "ur_gripper_85_moveit_config/launch/start_moveit.launch",
"chars": 3491,
"preview": "<launch>\n <!-- Specific ur robot -->\n <arg name=\"ur_robot\" default=\"ur3\"/>\n\n <!-- By default, we do not start a datab"
},
{
"path": "ur_gripper_85_moveit_config/launch/trajectory_execution.launch.xml",
"chars": 1468,
"preview": "<launch>\n\n <!-- This file makes it easy to include the settings for trajectory execution -->\n\n <arg name=\"execution_t"
},
{
"path": "ur_gripper_85_moveit_config/launch/ur_robot_gazebo_moveit_controller_manager.launch.xml",
"chars": 583,
"preview": "<launch>\n <arg name=\"execution_type\" default=\"interpolate\" />\n\n <!-- loads moveit_controller_manager on the parameter "
},
{
"path": "ur_gripper_85_moveit_config/launch/ur_robot_gazebo_moveit_sensor_manager.launch.xml",
"chars": 20,
"preview": "<launch>\n\n</launch>\n"
},
{
"path": "ur_gripper_85_moveit_config/launch/warehouse.launch",
"chars": 531,
"preview": "<launch>\n\n <!-- The path to the database must be specified -->\n <arg name=\"moveit_warehouse_database_path\" />\n\n <!-- "
},
{
"path": "ur_gripper_85_moveit_config/launch/warehouse_settings.launch.xml",
"chars": 675,
"preview": "<launch>\n <!-- Set the parameters for the warehouse and run the mongodb server. -->\n\n <!-- The default DB port for mov"
},
{
"path": "ur_gripper_85_moveit_config/package.xml",
"chars": 1576,
"preview": "<package>\n\n <name>ur_gripper_85_moveit_config</name>\n <version>0.3.0</version>\n <description>\n An automatically g"
},
{
"path": "ur_gripper_description/CMakeLists.txt",
"chars": 405,
"preview": "CMAKE_MINIMUM_REQUIRED(VERSION 3.1.3)\nPROJECT(ur_gripper_description)\n\nFIND_PACKAGE(catkin REQUIRED)\nCATKIN_PACKAGE()\n\nI"
},
{
"path": "ur_gripper_description/config/config.rviz",
"chars": 6441,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "ur_gripper_description/launch/bringup_with_gripper_85.launch",
"chars": 6032,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n \n <arg name=\"ur_robot\" default=\"ur3\"/>\n\n <arg name=\"debug\" default=\"false\" doc=\"Debug"
},
{
"path": "ur_gripper_description/launch/display_with_gripper_85.launch",
"chars": 892,
"preview": "<?xml version=\"1.0\"?>\r\n<launch>\r\n <arg name=\"gui\" default=\"True\" />\r\n \r\n <arg name=\"robot_description_file\" default=\""
},
{
"path": "ur_gripper_description/launch/display_with_gripper_hande.launch",
"chars": 895,
"preview": "<?xml version=\"1.0\"?>\r\n<launch>\r\n <arg name=\"gui\" default=\"true\" />\r\n\r\n <arg name=\"robot_description_file\" default=\"$("
},
{
"path": "ur_gripper_description/launch/load_ur_gripper_85.launch.xml",
"chars": 1461,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <!--Parameter files -->\n <arg name=\"ur_robot\" default=\"ur3\"/>\n \n <!--Common paramet"
},
{
"path": "ur_gripper_description/launch/load_ur_gripper_hande.launch.xml",
"chars": 1394,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <!--Parameter files -->\n <arg name=\"ur_robot\" default=\"ur3e\"/>\n\n <!--Common paramete"
},
{
"path": "ur_gripper_description/package.xml",
"chars": 763,
"preview": "<?xml version=\"1.0\"?>\n\n<package format=\"2\">\n <name>ur_gripper_description</name>\n <version>0.1.0</version>\n <descript"
},
{
"path": "ur_gripper_description/urdf/ur_gripper_85.xacro",
"chars": 2416,
"preview": "<?xml version=\"1.0\"?>\n<robot xmlns:xacro=\"http://wiki.ros.org/xacro\" name=\"ur_robot\">\n\n <xacro:include filename=\"$(find"
},
{
"path": "ur_gripper_description/urdf/ur_gripper_hande.xacro",
"chars": 2401,
"preview": "<?xml version=\"1.0\"?>\n<robot xmlns:xacro=\"http://wiki.ros.org/xacro\" name=\"ur_robot\">\n\n <xacro:include filename=\"$(find"
},
{
"path": "ur_gripper_gazebo/CMakeLists.txt",
"chars": 160,
"preview": "CMAKE_MINIMUM_REQUIRED(VERSION 3.1.3)\nPROJECT(ur_gripper_gazebo)\n\nFIND_PACKAGE(catkin REQUIRED)\n\ncatkin_python_setup()\n\n"
},
{
"path": "ur_gripper_gazebo/launch/gazebo_to_tf.launch",
"chars": 114,
"preview": "<launch>\n <node name=\"gazebo_to_tf\" pkg=\"ur_gripper_gazebo\" type=\"gazebo_to_tf.py\" output=\"screen\"/>\n</launch>"
},
{
"path": "ur_gripper_gazebo/launch/inc/load_ur_gripper_85.launch.xml",
"chars": 1598,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <!--Parameter files -->\n <arg name=\"ur_robot\" default=\"ur3\"/>\n <arg name=\"grasp_plug"
},
{
"path": "ur_gripper_gazebo/launch/inc/load_ur_gripper_hande.launch.xml",
"chars": 1602,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <!--Parameter files -->\n <arg name=\"ur_robot\" default=\"ur3e\"/>\n <arg name=\"grasp_plu"
},
{
"path": "ur_gripper_gazebo/launch/ur3_cubes_example.launch",
"chars": 172,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <include file=\"$(find ur_gripper_gazebo)/launch/ur_gripper_85_cubes.launch\">\n <arg n"
},
{
"path": "ur_gripper_gazebo/launch/ur3e_cubes_example.launch",
"chars": 176,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <include file=\"$(find ur_gripper_gazebo)/launch/ur_gripper_hande_cubes.launch\">\n <ar"
},
{
"path": "ur_gripper_gazebo/launch/ur3e_with_gripper.launch",
"chars": 1708,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- Export env variable so that gazebo finds our models -->\n <env name=\"GAZEBO_MODEL_"
},
{
"path": "ur_gripper_gazebo/launch/ur5_cubes_example.launch",
"chars": 172,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <include file=\"$(find ur_gripper_gazebo)/launch/ur_gripper_85_cubes.launch\">\n <arg n"
},
{
"path": "ur_gripper_gazebo/launch/ur_gripper_85_cubes.launch",
"chars": 2162,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- Export env variable so that gazebo finds our models -->\n <env name=\"GAZEBO_MODEL_"
},
{
"path": "ur_gripper_gazebo/launch/ur_gripper_hande_cubes.launch",
"chars": 2171,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <!-- Export env variable so that gazebo finds our models -->\n <env name=\"GAZEBO_MODEL_"
},
{
"path": "ur_gripper_gazebo/models/floor/materials/scripts/checkboard.material",
"chars": 287,
"preview": "material CheckerTexture\n{\n technique\n {\n pass\n {\n texture_unit\n {\n // Relative to the location "
},
{
"path": "ur_gripper_gazebo/models/floor/model.config",
"chars": 284,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Floor</name>\n <version>1.0</version>\n <sdf version=\"1.5\">model.sdf</sdf>\n\n <au"
},
{
"path": "ur_gripper_gazebo/models/floor/model.sdf",
"chars": 1034,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.5\">\n <model name=\"floor\">\n <static>true</static>\n <link name=\"link\">\n "
},
{
"path": "ur_gripper_gazebo/models/green_table/materials/scripts/repeated.material",
"chars": 291,
"preview": "material RepeatedTexture\n{\n technique\n {\n pass\n {\n texture_unit\n {\n // Relative to the location"
},
{
"path": "ur_gripper_gazebo/models/green_table/model.config",
"chars": 279,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>green_table</name>\n <version>1.0</version>\n <sdf version=\"1.6\">model.sdf</sdf>\n"
},
{
"path": "ur_gripper_gazebo/models/green_table/model.sdf",
"chars": 3090,
"preview": "<?xml version=\"1.0\"?>\n\n<sdf version=\"1.6\">\n <model name=\"green_table\">\n <static>true</static>\n\n <link name=\"top_p"
},
{
"path": "ur_gripper_gazebo/models/multi_peg_board/meshes/board.dae",
"chars": 574453,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "ur_gripper_gazebo/models/multi_peg_board/model.config",
"chars": 282,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Peg board</name>\n <version>1.0</version>\n <sdf version='1.5'>model.sdf</sdf>\n\n "
},
{
"path": "ur_gripper_gazebo/models/multi_peg_board/model.sdf",
"chars": 3142,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='multi_peg_board'>\n <static>true</static>\n <link name='boa"
},
{
"path": "ur_gripper_gazebo/models/peg_board/model.config",
"chars": 282,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Peg board</name>\n <version>1.0</version>\n <sdf version='1.5'>model.sdf</sdf>\n\n "
},
{
"path": "ur_gripper_gazebo/models/peg_board/model.sdf",
"chars": 3079,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='peg_board'>\n <static>true</static>\n <link name='board'>\n "
},
{
"path": "ur_gripper_gazebo/models/simple_peg_board/meshes/simple_board.dae",
"chars": 24396,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "ur_gripper_gazebo/models/simple_peg_board/model.config",
"chars": 296,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Simple Peg board</name>\n <version>1.0</version>\n <sdf version='1.5'>model.sdf</"
},
{
"path": "ur_gripper_gazebo/models/simple_peg_board/model.sdf",
"chars": 2761,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='simple_peg_board'>\n <static>true</static>\n <link name='bo"
},
{
"path": "ur_gripper_gazebo/models/ur3_base/meshes/dualbase.dae",
"chars": 1612623,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "ur_gripper_gazebo/models/ur3_base/model-bunri.sdf",
"chars": 5024,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='ur3_base'>\n <link name='base'>\n <pose frame=''>0 0 0 0 "
},
{
"path": "ur_gripper_gazebo/models/ur3_base/model.config",
"chars": 237,
"preview": "<?xml version=\"1.0\" ?>\n<model>\n <name>ur3_base</name>\n <version>1.0</version>\n <sdf version=\"1.6\">model.sdf</sd"
},
{
"path": "ur_gripper_gazebo/models/ur3_base/model.sdf",
"chars": 5024,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='ur3_base'>\n <link name='base'>\n <pose frame=''>0 0 0 0 "
},
{
"path": "ur_gripper_gazebo/models/ur3_table/model.config",
"chars": 238,
"preview": "<?xml version=\"1.0\" ?>\n<model>\n <name>ur3_table</name>\n <version>1.0</version>\n <sdf version=\"1.6\">model.sdf</s"
},
{
"path": "ur_gripper_gazebo/models/ur3_table/model.sdf",
"chars": 3201,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='ur3_table'>\n <link name='table'>\n <pose frame=''>0.06 0"
},
{
"path": "ur_gripper_gazebo/models/ur3e_base/materials/scripts/repeated.material",
"chars": 291,
"preview": "material RepeatedTexture\n{\n technique\n {\n pass\n {\n texture_unit\n {\n // Relative to the location"
},
{
"path": "ur_gripper_gazebo/models/ur3e_base/meshes/ur3e_base.dae",
"chars": 1612623,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "ur_gripper_gazebo/models/ur3e_base/model.config",
"chars": 238,
"preview": "<?xml version=\"1.0\" ?>\n<model>\n <name>ur3e_base</name>\n <version>1.0</version>\n <sdf version=\"1.6\">model.sdf</s"
},
{
"path": "ur_gripper_gazebo/models/ur3e_base/model.sdf",
"chars": 2773,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='ur3e_base'>\n <link name='base'>\n <pose frame=''>0 0 0 0"
},
{
"path": "ur_gripper_gazebo/models/ur3e_base/model_base.sdf",
"chars": 5088,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='ur3e_base'>\n <link name='base'>\n <pose frame=''>-0.15 -"
},
{
"path": "ur_gripper_gazebo/models/ur3e_table/model.config",
"chars": 239,
"preview": "<?xml version=\"1.0\" ?>\n<model>\n <name>ur3e_table</name>\n <version>1.0</version>\n <sdf version=\"1.6\">model.sdf</"
},
{
"path": "ur_gripper_gazebo/models/ur3e_table/model.sdf",
"chars": 2864,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='ur3e_table'>\n <link name='table'>\n <pose frame=''>0 0 0"
},
{
"path": "ur_gripper_gazebo/models/ur_base/model.config",
"chars": 264,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>ur_base</name>\n <version>1.0</version>\n <sdf version=\"1.6\">model.sdf</sdf>\n\n <"
},
{
"path": "ur_gripper_gazebo/models/ur_base/model.sdf",
"chars": 3269,
"preview": "<?xml version=\"1.0\"?>\n\n<sdf version=\"1.6\">\n <model name=\"ur_base\">\n <static>true</static>\n\n <link name=\"top_plate"
},
{
"path": "ur_gripper_gazebo/models/visual_marker/model.config",
"chars": 286,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Visual marker</name>\n <version>1.0</version>\n <sdf version='1.5'>model.sdf</sdf"
},
{
"path": "ur_gripper_gazebo/models/visual_marker/model.sdf",
"chars": 1777,
"preview": "<?xml version='1.0'?>\n<sdf version='1.6'>\n <model name='visual_marker'>\n <static>true</static>\n <link name='marke"
},
{
"path": "ur_gripper_gazebo/models/wood_cube/model.config",
"chars": 292,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Wood cube 10cm</name>\n <version>1.0</version>\n <sdf version='1.5'>model.sdf</sd"
},
{
"path": "ur_gripper_gazebo/models/wood_cube/model.rsdf",
"chars": 1451,
"preview": "<?xml version=\"1.0\" ?>\n<%\n # Wood block with dimensions 10 x 10 x 10 cm\n # SI units (length in meters)\n\n # Geometry\n "
},
{
"path": "ur_gripper_gazebo/models/wood_cube/model.sdf",
"chars": 1749,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.5\">\n <model name=\"wood_cube_10cm\">\n <link name=\"link\">\n <pose>0 0 0.05 0"
},
{
"path": "ur_gripper_gazebo/models/wooden_peg/model.config",
"chars": 287,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>Wooden Peg</name>\n <version>1.0</version>\n <sdf version='1.5'>model.sdf</sdf>\n\n"
},
{
"path": "ur_gripper_gazebo/models/wooden_peg/model.rsdf",
"chars": 1293,
"preview": "<?xml version=\"1.0\" ?>\n<%\n # SI units (length in meters)\n\n # Geometry\n # Height\n h = 0.08\n # diameter\n d = 0.0195"
},
{
"path": "ur_gripper_gazebo/models/wooden_peg/model.sdf",
"chars": 1845,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.5\">\n <model name=\"wooden_peg\">\n <link name=\"link\">\n <pose>0 0 0.04 0 0 0"
},
{
"path": "ur_gripper_gazebo/package.xml",
"chars": 651,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>ur_gripper_gazebo</name>\n <version>0.1.2</version>\n <description>\n "
},
{
"path": "ur_gripper_gazebo/scripts/gazebo_to_tf.py",
"chars": 1110,
"preview": "#!/usr/bin/python\nimport rospy\n\nfrom gazebo_msgs.msg import ModelStates\n\nimport tf\nimport rospy\n\nfrom ur_control import "
},
{
"path": "ur_gripper_gazebo/scripts/spawner.py",
"chars": 4243,
"preview": "#!/usr/bin/python3\n# Example of adding/removing models to gazebo simulator\nimport argparse\nimport rospy\nimport numpy as "
},
{
"path": "ur_gripper_gazebo/scripts/world_publisher.py",
"chars": 1769,
"preview": "#!/usr/bin/env python3\n# tf2 workaround for Python3\nimport sys\nsys.path[:0] = ['/usr/local/lib/python3.6/dist-packages/'"
},
{
"path": "ur_gripper_gazebo/setup.py",
"chars": 218,
"preview": "#!/usr/bin/env python\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\nd "
},
{
"path": "ur_gripper_gazebo/src/ur_gazebo/__init__.py",
"chars": 1,
"preview": "\n"
},
{
"path": "ur_gripper_gazebo/src/ur_gazebo/basic_models.py",
"chars": 7823,
"preview": "SPHERE = '<?xml version=\"1.0\" ?> \\\n<sdf version=\"1.5\"> \\\n <model name=\"%s\"> \\\n <static>true</static> \\\n <link nam"
},
{
"path": "ur_gripper_gazebo/src/ur_gazebo/gazebo_spawner.py",
"chars": 4460,
"preview": "import copy\nimport rospy\nimport rospkg\n\nfrom gazebo_msgs.msg import ModelStates, ModelState\nfrom gazebo_msgs.srv import "
},
{
"path": "ur_gripper_gazebo/src/ur_gazebo/model.py",
"chars": 1702,
"preview": "import copy\nfrom ur_control import transformations\nfrom geometry_msgs.msg import (\n Pose,\n Point,\n Quaternion\n)"
},
{
"path": "ur_gripper_gazebo/urdf/ur_gripper_85.xacro",
"chars": 7110,
"preview": "<?xml version=\"1.0\"?>\n<robot xmlns:xacro=\"http://wiki.ros.org/xacro\" name=\"ur_robot_gazebo\">\n <!--\n This is a top-le"
},
{
"path": "ur_gripper_gazebo/urdf/ur_gripper_hande.xacro",
"chars": 7066,
"preview": "<?xml version=\"1.0\"?>\n<robot xmlns:xacro=\"http://wiki.ros.org/xacro\" name=\"ur_robot_gazebo\">\n <!--\n This is a top-le"
},
{
"path": "ur_gripper_gazebo/worlds/cubes_task.world",
"chars": 13427,
"preview": "<?xml version=\"1.0\" ?>\n\n<sdf version=\"1.6\">\n\n <world name=\"ur3_cubes\">\n \n <gui>\n <camera name='user_camera'>"
},
{
"path": "ur_gripper_gazebo/worlds/ur3e.world",
"chars": 1399,
"preview": "<?xml version=\"1.0\" ?>\n\n<sdf version=\"1.6\">\n\n <world name=\"ur3e\">\n \n <gui>\n <camera name='user_camera'>\n "
},
{
"path": "ur_hande_moveit_config/.setup_assistant",
"chars": 934,
"preview": "moveit_setup_assistant_config:\n URDF:\n package: ur_gripper_gazebo\n relative_path: urdf/ur_gripper_hande.xacro\n "
},
{
"path": "ur_hande_moveit_config/CMakeLists.txt",
"chars": 311,
"preview": "cmake_minimum_required(VERSION 3.1.3)\nproject(ur_hande_moveit_config)\n\nfind_package(catkin REQUIRED)\n\ncatkin_package()\n\n"
},
{
"path": "ur_hande_moveit_config/config/cartesian_limits.yaml",
"chars": 99,
"preview": "cartesian_limits:\n max_trans_vel: 1\n max_trans_acc: 2.25\n max_trans_dec: -5\n max_rot_vel: 1.57\n"
},
{
"path": "ur_hande_moveit_config/config/chomp_planning.yaml",
"chars": 486,
"preview": "planning_time_limit: 10.0\nmax_iterations: 200\nmax_iterations_after_collision_free: 5\nsmoothness_cost_weight: 0.1\nobstacl"
},
{
"path": "ur_hande_moveit_config/config/fake_controllers.yaml",
"chars": 419,
"preview": "controller_list:\n - name: fake_arm_controller\n type: $(arg execution_type)\n joints:\n - shoulder_pan_joint\n "
},
{
"path": "ur_hande_moveit_config/config/joint_limits.yaml",
"chars": 1446,
"preview": "# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed\n# Spec"
},
{
"path": "ur_hande_moveit_config/config/kinematics.yaml",
"chars": 155,
"preview": "arm:\n kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin\n kinematics_solver_search_resolution: 0.005"
},
{
"path": "ur_hande_moveit_config/config/ompl_planning.yaml",
"chars": 8785,
"preview": "planner_configs:\n SBL:\n type: geometric::SBL\n range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0"
},
{
"path": "ur_hande_moveit_config/config/ros_controllers.yaml",
"chars": 1188,
"preview": "# Simulation settings for using moveit_sim_controllers\nmoveit_sim_hw_interface:\n joint_model_group: todo_group_name\n j"
},
{
"path": "ur_hande_moveit_config/config/sensors_3d.yaml",
"chars": 105,
"preview": "# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it\nsensors:\n - {}"
},
{
"path": "ur_hande_moveit_config/config/ur3e.urdf",
"chars": 58333,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\" ?>\n<!-- ==========================================================================="
},
{
"path": "ur_hande_moveit_config/config/ur3e_hande.urdf",
"chars": 19537,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<!-- ============================================================================"
},
{
"path": "ur_hande_moveit_config/config/ur_robot_gazebo.srdf",
"chars": 4884,
"preview": "<?xml version=\"1.0\" ?>\n<!--This does not replace URDF, and is not an extension of URDF.\n This is a format for represe"
},
{
"path": "ur_hande_moveit_config/launch/chomp_planning_pipeline.launch.xml",
"chars": 1410,
"preview": "<launch>\n <!-- CHOMP Plugin for MoveIt! -->\n <arg name=\"planning_plugin\" value=\"chomp_interface/CHOMPPlanner\" />\n\n <!"
},
{
"path": "ur_hande_moveit_config/launch/default_warehouse_db.launch",
"chars": 717,
"preview": "<launch>\n\n <arg name=\"reset\" default=\"false\"/>\n <!-- If not specified, we'll use a default database location -->\n <ar"
},
{
"path": "ur_hande_moveit_config/launch/demo.launch",
"chars": 3108,
"preview": "<launch>\n\n <!-- specify the planning pipeline -->\n <arg name=\"pipeline\" default=\"ompl\" />\n\n <!-- By default, we do no"
},
{
"path": "ur_hande_moveit_config/launch/demo_gazebo.launch",
"chars": 3438,
"preview": "<launch>\n\n <!-- By default, we do not start a database (it can be large) -->\n <arg name=\"db\" default=\"false\" />\n <!--"
},
{
"path": "ur_hande_moveit_config/launch/fake_moveit_controller_manager.launch.xml",
"chars": 547,
"preview": "<launch>\n\n <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->\n <arg n"
},
{
"path": "ur_hande_moveit_config/launch/gazebo.launch",
"chars": 988,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"paused\" default=\"false\"/>\n <arg name=\"gazebo_gui\" default=\"true\"/>\n <arg n"
},
{
"path": "ur_hande_moveit_config/launch/gazebo_static_tf.launch",
"chars": 270,
"preview": "<launch>\n <node pkg=\"tf\" type=\"static_transform_publisher\" name=\"floorworld_broadcaster\" args=\"0 0 0 0 0 0 floor worl"
},
{
"path": "ur_hande_moveit_config/launch/joystick_control.launch",
"chars": 621,
"preview": "<launch>\n <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->\n\n <arg name=\"dev\" default=\"/dev/inp"
},
{
"path": "ur_hande_moveit_config/launch/move_group.launch",
"chars": 4253,
"preview": "<launch>\n <!-- Specific ur robot -->\n <arg name=\"ur_robot\" default=\"ur3e\"/>\n\n <!-- GDB Debug Option -->\n <arg name=\""
},
{
"path": "ur_hande_moveit_config/launch/moveit.rviz",
"chars": 19011,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 84\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "ur_hande_moveit_config/launch/moveit_rviz.launch",
"chars": 576,
"preview": "<launch>\n\n <arg name=\"debug\" default=\"false\" />\n <arg unless=\"$(arg debug)\" name=\"launch_prefix\" value=\"\" />\n <arg "
},
{
"path": "ur_hande_moveit_config/launch/ompl_planning_pipeline.launch.xml",
"chars": 1314,
"preview": "<launch>\n\n <!-- OMPL Plugin for MoveIt! -->\n <arg name=\"planning_plugin\" value=\"ompl_interface/OMPLPlanner\" />\n\n <!--"
},
{
"path": "ur_hande_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml",
"chars": 1211,
"preview": "<launch>\n\n <!-- Pilz Command Planner Plugin for MoveIt -->\n <arg name=\"planning_plugin\" value=\"pilz_industrial_motion_"
},
{
"path": "ur_hande_moveit_config/launch/planning_context.launch",
"chars": 2783,
"preview": "<launch>\n <arg name=\"ur_robot\" default=\"ur3e\"/>\n\n <!-- By default we do not overwrite the URDF. Change the following t"
},
{
"path": "ur_hande_moveit_config/launch/planning_pipeline.launch.xml",
"chars": 690,
"preview": "<launch>\n\n <!-- This file makes it easy to include different planning pipelines;\n It is assumed that all planning"
},
{
"path": "ur_hande_moveit_config/launch/ros_controllers.launch",
"chars": 374,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <!-- Load joint controller configurations from YAML file to parameter server -->\n <ro"
},
{
"path": "ur_hande_moveit_config/launch/run_benchmark_ompl.launch",
"chars": 820,
"preview": "<launch>\n\n <!-- This argument must specify the list of .cfg files to process for benchmarking -->\n <arg name=\"cfg\" />\n"
},
{
"path": "ur_hande_moveit_config/launch/sensor_manager.launch.xml",
"chars": 785,
"preview": "<launch>\n\n <!-- This file makes it easy to include the settings for sensor managers -->\n\n <!-- Params for 3D sensors c"
},
{
"path": "ur_hande_moveit_config/launch/setup_assistant.launch",
"chars": 555,
"preview": "<!-- Re-launch the MoveIt! Setup Assistant with this configuration package already loaded -->\n<launch>\n\n <!-- Debug Inf"
},
{
"path": "ur_hande_moveit_config/launch/start_moveit.launch",
"chars": 3454,
"preview": "<launch>\n <!-- Specific ur robot -->\n <arg name=\"ur_robot\" default=\"ur3e\"/>\n\n <!-- By default, we do not start a data"
},
{
"path": "ur_hande_moveit_config/launch/trajectory_execution.launch.xml",
"chars": 1463,
"preview": "<launch>\n\n <!-- This file makes it easy to include the settings for trajectory execution -->\n\n <arg name=\"execution_t"
},
{
"path": "ur_hande_moveit_config/launch/ur_robot_gazebo_moveit_controller_manager.launch.xml",
"chars": 578,
"preview": "<launch>\n <arg name=\"execution_type\" default=\"interpolate\" />\n\n <!-- loads moveit_controller_manager on the parameter "
},
{
"path": "ur_hande_moveit_config/launch/ur_robot_gazebo_moveit_sensor_manager.launch.xml",
"chars": 20,
"preview": "<launch>\n\n</launch>\n"
},
{
"path": "ur_hande_moveit_config/launch/warehouse.launch",
"chars": 526,
"preview": "<launch>\n\n <!-- The path to the database must be specified -->\n <arg name=\"moveit_warehouse_database_path\" />\n\n <!-- "
},
{
"path": "ur_hande_moveit_config/launch/warehouse_settings.launch.xml",
"chars": 675,
"preview": "<launch>\n <!-- Set the parameters for the warehouse and run the mongodb server. -->\n\n <!-- The default DB port for mov"
},
{
"path": "ur_hande_moveit_config/package.xml",
"chars": 1571,
"preview": "<package>\n\n <name>ur_hande_moveit_config</name>\n <version>0.3.0</version>\n <description>\n An automatically genera"
}
]
// ... and 33 more files (download for full content)
About this extraction
This page contains the full source code of the cambel/ur3 GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 233 files (40.7 MB), approximately 1.2M tokens, and a symbol index with 629 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.