├── .docker ├── ci-shadow-fixed │ └── Dockerfile ├── ci │ └── Dockerfile └── source │ └── Dockerfile ├── .gitignore ├── .travis.yml ├── .travis_ci ├── travis.sh └── util.sh ├── README.md ├── sns_ik ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── sns_ik_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ ├── example_baxter_joint_limits.yaml │ └── example_sawyer_joint_limits.yaml ├── launch │ ├── test_baxter.launch │ ├── test_ik_solvers.launch │ └── test_sawyer.launch ├── package.xml └── src │ └── ik_tests.cpp ├── sns_ik_kinematics_plugin ├── CATKIN_IGNORE ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md └── package.xml └── sns_ik_lib ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE.txt ├── include └── sns_ik │ ├── fosns_velocity_ik.hpp │ ├── fsns_velocity_ik.hpp │ ├── osns_sm_velocity_ik.hpp │ ├── osns_velocity_ik.hpp │ ├── sns_acc_ik_base.hpp │ ├── sns_ik.hpp │ ├── sns_ik_base.hpp │ ├── sns_position_ik.hpp │ ├── sns_vel_ik_base.hpp │ ├── sns_vel_ik_base_interface.hpp │ └── sns_velocity_ik.hpp ├── matlab ├── FindScaleFactor.m ├── LICENSE.txt ├── README.md ├── TEST_acc_ik_solvers.m ├── TEST_vel_ik_solvers.m ├── runTest_snsIk_acc.m ├── runTest_snsIk_acc_cs.m ├── runTest_snsIk_acc_mt.m ├── runTest_snsIk_vel.m ├── runTest_snsIk_vel_cs.m ├── runTest_snsIk_vel_mt.m ├── snsIk_acc_QP.m ├── snsIk_acc_QP_mt.m ├── snsIk_acc_opt.m ├── snsIk_acc_rr.m ├── snsIk_acc_rr_cs.m ├── snsIk_acc_rr_mt.m ├── snsIk_vel_QP.m ├── snsIk_vel_QP_mt.m ├── snsIk_vel_basic.m ├── snsIk_vel_opt.m ├── snsIk_vel_rr.m ├── snsIk_vel_rr_cs.m └── snsIk_vel_rr_mt.m ├── package.xml ├── src ├── fosns_velocity_ik.cpp ├── fsns_velocity_ik.cpp ├── osns_sm_velocity_ik.cpp ├── osns_velocity_ik.cpp ├── sns_acc_ik_base.cpp ├── sns_ik.cpp ├── sns_ik_base.cpp ├── sns_position_ik.cpp ├── sns_vel_ik_base.cpp ├── sns_vel_ik_base_interface.cpp └── sns_velocity_ik.cpp ├── test ├── rng_utilities.cpp ├── rng_utilities.hpp ├── rng_utilities_test.cpp ├── sawyer_model.cpp ├── sawyer_model.hpp ├── sns_acc_ik_base_test.cpp ├── sns_ik_math_utils_test.cpp ├── sns_ik_pos_test.cpp ├── sns_ik_vel_test.cpp ├── sns_vel_ik_base_test.cpp ├── test_utilities.cpp └── test_utilities.hpp └── utilities ├── sns_ik_math_utils.cpp ├── sns_ik_math_utils.hpp ├── sns_linear_solver.cpp └── sns_linear_solver.hpp /.docker/ci-shadow-fixed/Dockerfile: -------------------------------------------------------------------------------- 1 | # rethinkroboticsopensource/sns-ik:indigo-ci-shadow-fixed 2 | # Sets up a base image to use for running Continuous Integration on Travis 3 | 4 | FROM rethinkroboticsopensource/sns-ik:indigo-ci 5 | MAINTAINER Ian McMahon git@ianthe.engineer 6 | 7 | # Switch to ros-shadow-fixed 8 | RUN echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main" | tee /etc/apt/sources.list.d/ros-latest.list 9 | 10 | # Upgrade packages to ros-shadow-fixed and clean apt-cache within one RUN command 11 | RUN apt-get -qq update && \ 12 | apt-get -qq dist-upgrade && \ 13 | # Clear apt-cache to reduce image size 14 | rm -rf /var/lib/apt/lists/* 15 | -------------------------------------------------------------------------------- /.docker/ci/Dockerfile: -------------------------------------------------------------------------------- 1 | # rethinkroboticsopensource/sns-ik:indigo-ci 2 | # Sets up a base image to use for running Continuous Integration on Travis 3 | 4 | FROM ros:indigo-ros-base 5 | MAINTAINER Ian McMahon git@ianthe.engineer 6 | 7 | ENV TERM xterm 8 | 9 | # Setup catkin workspace 10 | ENV CATKIN_WS=/root/ws_sns_ik 11 | ENV ROS_DISTRO=indigo 12 | WORKDIR $CATKIN_WS 13 | # Continous Integration Setting 14 | ENV IN_DOCKER 1 15 | 16 | # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size 17 | # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers 18 | RUN \ 19 | mkdir src && \ 20 | cd src && \ 21 | 22 | # Download source so that we can get necessary dependencies 23 | git clone https://github.com/RethinkRobotics-opensource/sns_ik.git && \ 24 | cd sns_ik && \ 25 | git checkout ${ROS_DISTRO}-devel && \ 26 | cd .. && \ 27 | 28 | # Update apt package list as previous containers clear the cache 29 | apt-get -qq update && \ 30 | apt-get -qq dist-upgrade && \ 31 | 32 | # Install some base dependencies 33 | apt-get -qq install -y \ 34 | # Some source builds require a package.xml be downloaded via wget from an external location 35 | wget \ 36 | # Required for rosdep command 37 | sudo \ 38 | # Preferred build tools 39 | python-catkin-tools \ 40 | build-essential \ 41 | ccache && \ 42 | 43 | # Download all dependencies 44 | rosdep update && \ 45 | rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ 46 | 47 | # Remove the source code from this container. TODO: in the future we may want to keep this here for further optimization of later containers 48 | cd .. && \ 49 | rm -rf src/ && \ 50 | 51 | # Clear apt-cache to reduce image size 52 | rm -rf /var/lib/apt/lists/* 53 | 54 | 55 | # Continous Integration Setting 56 | ENV IN_DOCKER 1 57 | -------------------------------------------------------------------------------- /.docker/source/Dockerfile: -------------------------------------------------------------------------------- 1 | # rethinkroboticsopensource/sns-ik:indigo-source 2 | # Downloads the moveit source code, install remaining debian dependencies, and builds workspace 3 | 4 | FROM rethinkroboticsopensource/sns-ik:indigo-ci-shadow-fixed 5 | MAINTAINER Ian McMahon git@ianthe.engineer 6 | 7 | # Setup catkin workspace 8 | ENV CATKIN_WS=/root/ws_sns_ik 9 | ENV ROS_DISTRO=indigo 10 | WORKDIR $CATKIN_WS 11 | 12 | # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size 13 | # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers 14 | RUN \ 15 | mkdir src && \ 16 | cd src && \ 17 | 18 | # Download source so that we can get necessary dependencies 19 | git clone https://github.com/RethinkRobotics-opensource/sns_ik.git && \ 20 | cd sns_ik && \ 21 | git checkout ${ROS_DISTRO}-devel && \ 22 | cd .. && \ 23 | 24 | # Update apt package list as previous containers clear the cache 25 | apt-get -qq update && \ 26 | apt-get -qq dist-upgrade && \ 27 | 28 | # Install some base dependencies 29 | apt-get -qq install -y \ 30 | # Some source builds require a package.xml be downloaded via wget from an external location 31 | wget \ 32 | # Required for rosdep command 33 | sudo \ 34 | # Preferred build tools 35 | python-catkin-tools \ 36 | build-essential \ 37 | ccache && \ 38 | 39 | # Download all dependencies 40 | rosdep update && \ 41 | rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ 42 | 43 | # Replacing shell with bash for later docker build commands 44 | mv /bin/sh /bin/sh-old && \ 45 | ln -s /bin/bash /bin/sh 46 | 47 | # Build repo 48 | WORKDIR $CATKIN_WS 49 | ENV TERM xterm 50 | ENV PYTHONIOENCODING UTF-8 51 | RUN catkin config --extend /opt/ros/$ROS_DISTRO --install --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 52 | catkin build 53 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # Vim artifacts 31 | *~ 32 | *.swp 33 | *.swo 34 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes 2 | sudo: required 3 | dist: trusty 4 | services: 5 | - docker 6 | language: cpp 7 | cache: ccache 8 | compiler: gcc 9 | 10 | notifications: 11 | email: 12 | recipients: 13 | - git@ianthe.engineer 14 | env: 15 | matrix: 16 | - ROS_DISTRO=indigo ROS_REPO=ros 17 | - ROS_DISTRO=indigo ROS_REPO=ros-shadow-fixed 18 | script: 19 | - .travis_ci/travis.sh 20 | -------------------------------------------------------------------------------- /.travis_ci/travis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Software License Agreement - BSD License 4 | # 5 | # Inspired by MoveIt! travis https://github.com/ros-planning/moveit_core/blob/09bbc196dd4388ac8d81171620c239673b624cc4/.travis.yml 6 | # Inspired by JSK travis https://github.com/jsk-ros-pkg/jsk_travis 7 | # Inspired by ROS Industrial https://github.com/ros-industrial/industrial_ci 8 | # 9 | # Maintainer: Ian McMahon 10 | # Author: Dave Coleman, Isaac I. Y. Saito, Robert Haschke 11 | 12 | # Note: ROS_REPOSITORY_PATH is no longer a valid option, use ROS_REPO. See README.md 13 | 14 | export CI_SOURCE_PATH=$(pwd) # The repository code in this pull request that we are testing 15 | export CI_PARENT_DIR=.travis_ci # This is the folder name that is used in downstream repositories in order to point to this repo. 16 | export HIT_ENDOFSCRIPT=false 17 | export REPOSITORY_NAME=${PWD##*/} 18 | export CATKIN_WS=/root/ws_sns_ik 19 | echo "---" 20 | echo "Testing branch '$TRAVIS_BRANCH' of '$REPOSITORY_NAME' on ROS '$ROS_DISTRO'" 21 | 22 | # Helper functions 23 | source ${CI_SOURCE_PATH}/$CI_PARENT_DIR/util.sh 24 | 25 | # Run all CI in a Docker container 26 | if ! [ "$IN_DOCKER" ]; then 27 | 28 | # Choose the correct CI container to use 29 | case "$ROS_REPO" in 30 | ros-shadow-fixed) 31 | export DOCKER_IMAGE=rethinkroboticsopensource/sns-ik:$ROS_DISTRO-ci-shadow-fixed 32 | ;; 33 | *) 34 | export DOCKER_IMAGE=rethinkroboticsopensource/sns-ik:$ROS_DISTRO-ci 35 | ;; 36 | esac 37 | echo "Starting Docker image: $DOCKER_IMAGE" 38 | 39 | # Pull first to allow us to hide console output 40 | docker pull $DOCKER_IMAGE > /dev/null 41 | 42 | # Start Docker container 43 | docker run \ 44 | -e ROS_REPO \ 45 | -e ROS_DISTRO \ 46 | -e BEFORE_SCRIPT \ 47 | -e CI_PARENT_DIR \ 48 | -e CI_SOURCE_PATH \ 49 | -e UPSTREAM_WORKSPACE \ 50 | -e TRAVIS_BRANCH \ 51 | -e TEST \ 52 | -e TEST_BLACKLIST \ 53 | -v $(pwd):/root/$REPOSITORY_NAME \ 54 | -v $HOME/.ccache:/root/.ccache \ 55 | $DOCKER_IMAGE \ 56 | /bin/bash -c "cd /root/$REPOSITORY_NAME; source .travis_ci/travis.sh;" 57 | return_value=$? 58 | 59 | if [ $return_value -eq 0 ]; then 60 | echo "$DOCKER_IMAGE container finished successfully" 61 | HIT_ENDOFSCRIPT=true; 62 | exit 0 63 | fi 64 | echo "$DOCKER_IMAGE container finished with errors" 65 | exit 1 # error 66 | fi 67 | 68 | # If we are here, we can assume we are inside a Docker container 69 | echo "Inside Docker container" 70 | 71 | # Update the sources 72 | travis_run apt-get -qq update 73 | 74 | # Make sure the packages are up-to-date 75 | travis_run apt-get -qq dist-upgrade 76 | 77 | # Enable ccache 78 | travis_run apt-get -qq install ccache 79 | export PATH=/usr/lib/ccache:$PATH 80 | 81 | # Install and run xvfb to allow for X11-based unittests on DISPLAY :99 82 | travis_run apt-get -qq install xvfb mesa-utils 83 | Xvfb -screen 0 640x480x24 :99 & 84 | export DISPLAY=:99.0 85 | travis_run_true glxinfo 86 | 87 | # Setup rosdep - note: "rosdep init" is already setup in base ROS Docker image 88 | travis_run rosdep update 89 | 90 | # Create workspace 91 | travis_run mkdir -p $CATKIN_WS/src 92 | travis_run cd $CATKIN_WS/src 93 | 94 | # Install dependencies necessary to run build using .rosinstall files 95 | if [ ! "$UPSTREAM_WORKSPACE" ]; then 96 | export UPSTREAM_WORKSPACE="debian"; 97 | fi 98 | 99 | # link in the repo we are testing 100 | travis_run ln -s $CI_SOURCE_PATH . 101 | 102 | # Debug: see the files in current folder 103 | travis_run ls -a 104 | 105 | # Run before script 106 | if [ "${BEFORE_SCRIPT// }" != "" ]; then 107 | travis_run sh -c "${BEFORE_SCRIPT}"; 108 | fi 109 | 110 | # Install source-based package dependencies 111 | travis_run rosdep install -y -q -n --from-paths . --ignore-src --rosdistro $ROS_DISTRO 112 | 113 | # Change to base of workspace 114 | travis_run cd $CATKIN_WS 115 | 116 | # Configure catkin 117 | travis_run catkin config --extend /opt/ros/$ROS_DISTRO --install --cmake-args -DCMAKE_BUILD_TYPE=Release 118 | 119 | # Console output fix for: "WARNING: Could not encode unicode characters" 120 | export PYTHONIOENCODING=UTF-8 121 | 122 | # For a command that doesn’t produce output for more than 10 minutes, prefix it with travis_run_wait 123 | travis_run_wait 60 catkin build --no-status --summarize || exit 1 124 | 125 | travis_run ccache -s 126 | 127 | # Source the new built workspace 128 | travis_run source install/setup.bash; 129 | 130 | # Choose which packages to run tests on 131 | echo "Test blacklist: $TEST_BLACKLIST" 132 | echo "--------------" 133 | TEST_PKGS=$(catkin_topological_order "$CI_SOURCE_PATH" --only-names | grep -Fvxf <(echo "$TEST_BLACKLIST" | tr ' ;,' '\n') | tr '\n' ' ') 134 | 135 | if [ -n "$TEST_PKGS" ]; then 136 | TEST_PKGS="--no-deps $TEST_PKGS"; 137 | # Fix formatting of list of packages to work correctly with Travis 138 | IFS=' ' read -r -a TEST_PKGS <<< "$TEST_PKGS" 139 | fi 140 | echo -e "Test packages: ${TEST_PKGS}" 141 | 142 | # Run catkin package tests 143 | travis_run catkin build --no-status --summarize --make-args tests -- ${TEST_PKGS[@]} 144 | 145 | # Run non-catkin package tests 146 | travis_run catkin build --catkin-make-args run_tests -- --no-status --summarize ${TEST_PKGS[@]} 147 | 148 | # Show test results and throw error if necessary 149 | travis_run catkin_test_results 150 | 151 | echo "Travis script has finished successfully" 152 | HIT_ENDOFSCRIPT=true 153 | exit 0 154 | -------------------------------------------------------------------------------- /.travis_ci/util.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #******************************************************************** 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2016, University of Colorado, Boulder 6 | # All rights reserved. 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions 10 | # are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above 15 | # copyright notice, this list of conditions and the following 16 | # disclaimer in the documentation and/or other materials provided 17 | # with the distribution. 18 | # * Neither the name of the Univ of CO, Boulder nor the names of its 19 | # contributors may be used to endorse or promote products derived 20 | # from this software without specific prior written permission. 21 | # 22 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | # POSSIBILITY OF SUCH DAMAGE. 34 | #********************************************************************/ 35 | 36 | # Author: Dave Coleman , Robert Haschke 37 | # Desc: Utility functions used to make CI work better in Travis 38 | 39 | ####################################### 40 | export TRAVIS_FOLD_COUNTER=0 41 | 42 | 43 | ####################################### 44 | # Start a Travis fold with timer 45 | # 46 | # Arguments: 47 | # travis_fold_name: name of line 48 | # command: action to run 49 | ####################################### 50 | function travis_time_start { 51 | TRAVIS_START_TIME=$(date +%s%N) 52 | TRAVIS_TIME_ID=$(cat /dev/urandom | tr -dc 'a-z0-9' | fold -w 8 | head -n 1) 53 | TRAVIS_FOLD_NAME=$1 54 | local COMMAND=${@:2} # all arguments except the first 55 | 56 | # Start fold 57 | echo -e "\e[0Ktravis_fold:start:$TRAVIS_FOLD_NAME" 58 | # Output command being executed 59 | echo -e "\e[0Ktravis_time:start:$TRAVIS_TIME_ID\e[34m$COMMAND\e[0m" 60 | } 61 | 62 | ####################################### 63 | # Wraps up the timer section on Travis CI (that's started mostly by travis_time_start function). 64 | # 65 | # Arguments: 66 | # travis_fold_name: name of line 67 | ####################################### 68 | function travis_time_end { 69 | if [ -z $TRAVIS_START_TIME ]; then 70 | echo '[travis_time_end] var TRAVIS_START_TIME is not set. You need to call `travis_time_start` in advance.'; 71 | return; 72 | fi 73 | local TRAVIS_END_TIME=$(date +%s%N) 74 | local TIME_ELAPSED_SECONDS=$(( ($TRAVIS_END_TIME - $TRAVIS_START_TIME)/1000000000 )) 75 | 76 | # Output Time 77 | echo -e "travis_time:end:$TRAVIS_TIME_ID:start=$TRAVIS_START_TIME,finish=$TRAVIS_END_TIME,duration=$(($TRAVIS_END_TIME - $TRAVIS_START_TIME))\e[0K" 78 | # End fold 79 | echo -e -n "travis_fold:end:$TRAVIS_FOLD_NAME\e[0m" 80 | 81 | unset TRAVIS_START_TIME 82 | unset TRAVIS_TIME_ID 83 | unset TRAVIS_FOLD_NAME 84 | } 85 | 86 | ####################################### 87 | # Display command in Travis console and fold output in dropdown section 88 | # 89 | # Arguments: 90 | # command: action to run 91 | ####################################### 92 | function travis_run_impl() { 93 | local command=$@ 94 | 95 | let "TRAVIS_FOLD_COUNTER += 1" 96 | travis_time_start travis_ci.$TRAVIS_FOLD_COUNTER $command 97 | # actually run command 98 | $command 99 | result=$? 100 | travis_time_end 101 | return $result 102 | } 103 | 104 | ####################################### 105 | # Run a command and do folding and timing for it 106 | # Return the exit status of the command 107 | function travis_run() { 108 | travis_run_impl $@ || exit $? 109 | } 110 | 111 | ####################################### 112 | # Same as travis_run but return 0 exit status, thus ignoring any error 113 | function travis_run_true() { 114 | travis_run_impl $@ || return 0 115 | } 116 | 117 | ####################################### 118 | # Same as travis_run, but issue some output regularly to indicate that the process is still alive 119 | # from: https://github.com/travis-ci/travis-build/blob/d63c9e95d6a2dc51ef44d2a1d96d4d15f8640f22/lib/travis/build/script/templates/header.sh 120 | function travis_run_wait() { 121 | local timeout=$1 # in minutes 122 | 123 | if [[ $timeout =~ ^[0-9]+$ ]]; then 124 | # looks like an integer, so we assume it's a timeout 125 | shift 126 | else 127 | # default value 128 | timeout=20 129 | fi 130 | 131 | local cmd=$@ 132 | let "TRAVIS_FOLD_COUNTER += 1" 133 | travis_time_start travis_ci.$TRAVIS_FOLD_COUNTER $cmd 134 | 135 | # Disable bash's job control messages 136 | set +m 137 | # Run actual command in background 138 | $cmd & 139 | local cmd_pid=$! 140 | 141 | travis_jigger $cmd_pid $timeout $cmd & 142 | local jigger_pid=$! 143 | local result 144 | 145 | { 146 | wait $cmd_pid 2>/dev/null 147 | result=$? 148 | # if process finished before jigger, stop the jigger too 149 | ps -p$jigger_pid 2>&1>/dev/null && kill $jigger_pid 150 | } 151 | 152 | echo 153 | travis_time_end 154 | 155 | return $result 156 | } 157 | 158 | ####################################### 159 | function travis_jigger() { 160 | local cmd_pid=$1 161 | shift 162 | local timeout=$1 163 | shift 164 | local count=0 165 | 166 | echo -n "Waiting for process to finish " 167 | while [ $count -lt $timeout ]; do 168 | count=$(($count + 1)) 169 | echo -ne "." 170 | sleep 60 # wait 60s 171 | done 172 | 173 | echo -e "\n\033[31;1mTimeout (${timeout} minutes) reached. Terminating \"$@\"\033[0m\n" 174 | kill -9 $cmd_pid 175 | } 176 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SNS-IK Library 2 | `Version 0.2.3 beta` 3 | 4 | The Saturation in the Null-Space (SNS) Inverse-Kinematics (IK) Library 5 | implements a collection of algorithms written by Fabrizio Flacco for 6 | inverting the differential kinematics of a robot. 7 | 8 | ### Continuous Integration Builds 9 | 10 | ROS Indigo | ROS Kinetic | ROS Melodic 11 | ---------- | ----------- | ----------- 12 | [![Build Status](https://travis-ci.org/RethinkRobotics-opensource/sns_ik.svg?branch=indigo-devel)](https://travis-ci.org/RethinkRobotics-opensource/sns_ik) | [![Build Status](https://travis-ci.org/RethinkRobotics-opensource/sns_ik.svg?branch=kinetic-devel)](https://travis-ci.org/RethinkRobotics-opensource/sns_ik) | [![Build Status](https://travis-ci.org/RethinkRobotics-opensource/sns_ik.svg?branch=melodic-devel)](https://travis-ci.org/RethinkRobotics-opensource/sns_ik) | 13 | 14 | ## What problems are solved by this library? 15 | 16 | The SNS-IK library is a library that is designed to compute fast solutions to 17 | inverse-kinematics problems on redundant kinematic chains. 18 | It is particularly good at handling multiple prioritized task objectives 19 | while satisfying joint position and velocity limits. 20 | 21 | The core solvers in this library operate at the velocity-level, although we 22 | also include a position-level solver. 23 | 24 | ## Algorithm Overview: 25 | 26 | **SNS Velocity IK:** This is the core algorithm developed by Fabrizio. 27 | All of the other algorithms in this library are improvements upon this one. 28 | 29 | **Optimal SNS:** Add an objective function to the standard SNS velocity IK solver, 30 | allowing it to compute a solution that is both feasible and optimal. 31 | 32 | **Optimal SNS with Margin:** Improvement upon the Optimal SNS solver to make it 33 | better at avoiding discontinuous velocities over a sequence of IK calls. 34 | 35 | **Fast SNS IK:** Several numerical improvements to reduce the total CPU time 36 | required for the SNS Velocity IK solver. 37 | 38 | **Fast Optimal SNS:** Similar to the Optimal SNS, but with several numerical improvements. 39 | 40 | **SNS Base Velocity/Acceleration IK w/ and w/o Configuration Task as Secondary Goal:** This uses SNS IK algorithms rewritten by Andy Park. 41 | These algorithms passed rigorous unit tests and they much more robust than the original algorithms developed by Fabrizio in edge cases. And by providing an acceleration-level IK, they result in inherently continuous velocity outputs. 42 | 43 | ## References: 44 | 45 | The algorithms in this library are drawn from three papers, 46 | all written by the same team of three authors: 47 | - Fabrizio Flacco 48 | - Alessandro De Luca 49 | - Oussama Khatib 50 | 51 | The primary reference is: 52 | - *Control of Redundant Robots Under Hard Joint Constraint: Saturation in the Null Space* 53 | ([.pdf](https://pdfs.semanticscholar.org/97ad/e6bad155d443e40f7b99d9773881b73a6ebc.pdf)) 54 | ([IEEE](https://ieeexplore.ieee.org/document/7097068/)). 55 | ([video](https://youtu.be/Zm60jBdP-xs)) 56 | 57 | These two earlier papers are also relevant: 58 | - *Prioritized multi-task motion control of redundant robots under hard joint constraints* 59 | ([.pdf](https://cs.stanford.edu/groups/manips/publications/pdfs/Flacco_2012.pdf)) 60 | ([IEEE](https://ieeexplore.ieee.org/document/6385619/)). 61 | - *Motion control of redundant robots under joint constraints: Saturation in the Null Space* 62 | ([.pdf](http://www.diag.uniroma1.it/~labrob/pub/papers/ICRA12_RedundancySNS.pdf)) 63 | ([IEEE](https://ieeexplore.ieee.org/document/6225376/)). 64 | 65 | ## Contributors 66 | 67 | **Original Library:** 68 | ``` 69 | Fabrizio Flacco 70 | Dipartimento di Ingegneria Informatica, Automatica e Gestionale (DIAG) 71 | Università di Roma "La Sapienza" 72 | Rome, Italy 73 | ``` 74 | **Maintainence and Updates:** 75 | ```` 76 | Forrest Rogers-Marcovitz, Ian McMahon, Matthew Kelly, and Andy Park 77 | Rethink Robotics 78 | Boston, MA, USA 79 | ```` 80 | -------------------------------------------------------------------------------- /sns_ik/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sns_ik 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.3 (2017-10-29) 6 | ------------------ 7 | * No Updates 8 | 9 | 0.2.1 (2016-10-25) 10 | ------------------ 11 | * No Updates 12 | 13 | 0.2.0 (2016-09-06) 14 | ------------------ 15 | * No updates 16 | 17 | 0.1.1 (2016-04-28) 18 | --------------------------------- 19 | * No updates 20 | 21 | 0.1.0 (2016-04-22) 22 | --------------------------------- 23 | * Converts sns lib into package framework 24 | This commit lays the foundation for a collection of 25 | SNS IK packages: 26 | - sns_ik : Metapackage for SNS IK solver, plugins, examples 27 | - sns_ik_lib: SNS IK solver library 28 | - sns_ik_examples: Tests and examples for SNS IK library 29 | - sns_ik_kinematic_plugins: A package for integrating SNS IK with moveit_ros 30 | Also, added Fabrezio, Ian, and Forrest as authors of these packages. 31 | * Contributors: Ian McMahon 32 | -------------------------------------------------------------------------------- /sns_ik/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sns_ik) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /sns_ik/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sns_ik 4 | 0.2.3 5 | 6 | Metapackage for the Saturation in the Null Space (SNS) 7 | Inverse Kinematic Library. 8 | 9 | 10 | Fabrizio Flacco 11 | Ian McMahon 12 | Forrest Rogers-Marcovitz 13 | 14 | Rethink Robotics Inc. 15 | 16 | Apache 2.0 17 | http://ros.org/wiki/sns_ikl 18 | 19 | https://github.com/RethinkRobotics-opensource/sns_ikl 20 | 21 | 22 | https://github.com/RethinkRobotics-opensource/sns_ikl/issues 23 | 24 | 25 | catkin 26 | 27 | sns_ik_lib 28 | sns_ik_examples 29 | 30 | 31 | -------------------------------------------------------------------------------- /sns_ik_examples/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sns_ik_examples 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.3 (2017-10-29) 6 | ------------------ 7 | * CMakeLists Eigen cleanup 8 | In ROS Kinetic, cmake_modules is deprecated, 9 | so we will use some alternative CMakeLists 10 | strategies to find_package the Eigen 3.x library. 11 | * Contributors: Ian McMahon 12 | 13 | 0.2.1 (2016-10-25) 14 | ------------------ 15 | * No Updates 16 | 17 | 0.2.0 (2016-09-06) 18 | ------------------ 19 | * Fixes for stricter build rules 20 | * Contributors: Forrest Rogers-Marcovitz 21 | 22 | 0.1.1 (2016-04-28) 23 | --------------------------------- 24 | * No updates 25 | 26 | 0.1.0 (2016-04-22) 27 | --------------------------------- 28 | * Fixed nullspace evalution bugs 29 | * Updated per review comments for Nullspace Tests 30 | * Added nullspace influence calculation 31 | * Added position limit checks on IK solvers 32 | * Added final pose checks for position solvers 33 | * Nullspace testing wip 34 | * Basic nullspace bias task functionality 35 | * Output time std deviation with plus/minus sign. Increased number of position tests. 36 | * Turned on compile optimization -O2. Huge speed increase ~20X 37 | * For the velocity solver in sns_ik, changed the name to CartToJntVel to minimize name confusion. 38 | * Merge with origin/master 39 | * First pass at nullspace bias tasks for position IK. Not optimized yet. 40 | * Added Pos Solver Time Standard Deviation 41 | This commit adds Standard Deviation calculation for 42 | the time each position solver takes to run, for KDL, 43 | Trac & all SNS solvers. 44 | * Added Launch file heierchy for multiple robots 45 | Now the test_ik_solvers.launch is expecting a URDF to 46 | have already been loaded into /robot_description or 47 | the specfied urdf_param location. 48 | * Standardized seed-testing interface 49 | * First pass at nullspace bias tasks for velocity IK. Fixed P matrix for secondary tasks in fsns and fosns. Also turned off singularity logging. 50 | * Updated based on review comments 51 | * Adds close delta seeds for Position IK Testing 52 | Adds a seed that is close to the random joint 53 | angle used in constructing an inverse kinematic solition. 54 | This should simulate the most favorable (and realistic) 55 | conditions for using the solver in cartesian movement. 56 | TBD: - Parametrize the 0.2 radian delta for the seed 57 | - Test how close the resulting joint solutions are to the delta 58 | seed 59 | * Better time comparison. 60 | * Allow random seeds and logging cleanup 61 | * Check rotational speed scaling 62 | * Cleaner test output. 63 | * Fixed Interface breakage & invalid function call 64 | * Updates Position IK interface based on review comments 65 | * Adds SharedPtr Get interface for Pos & Vel solvers 66 | * Additional tracking of if Cartesian velocities are scaled correctly. 67 | * Fast optimal SNS. There is still probably a bug in the code. 68 | * Fast SNS algorithm 69 | * A few minor changes with parameters and logging. Random seed based on time. Small parameter changes. 70 | * Added testing and infra for Velocity SNS tests 71 | * Adds SNS_IK() class for using URDF & limits 72 | This changes adds the SNS_IK class which will construct a 73 | KDL chain from the specified robot_description parameter. 74 | Position and velocity limits can be read from the URDF, and overridden 75 | by including a standard robot_description_planning/joint_limits yaml 76 | file of lower position, upper position, max velocity and max acceleration 77 | limits. 78 | Finally, a new testing executable has been included which is based largely 79 | off of trac_ik_examples tests. This will compare sns_ik against KDL and 80 | trac_ik position solvers. 81 | * Fixed Cartesian error math which was different from Eigen 82 | * Renamend sm class 83 | * Merged from master 84 | * Fixed a number of bugs in the position ik solver, though still not converging correctly 85 | * Added Optimal SNS velocity solver with scale margin 86 | * Correct inheritance 87 | * Created a class for the optimal SNS velocity IK solver 88 | * Fixed crash related to un-initialized variables 89 | * Attemp to add position ik test, but causing fault in velocity ik solver. 90 | * Better checks for input sizes 91 | * Merged in massive library formatting 92 | * Converts sns lib into package framework 93 | This commit lays the foundation for a collection of 94 | SNS IK packages: 95 | - sns_ik : Metapackage for SNS IK solver, plugins, examples 96 | - sns_ik_lib: SNS IK solver library 97 | - sns_ik_examples: Tests and examples for SNS IK library 98 | - sns_ik_kinematic_plugins: A package for integrating SNS IK with moveit_ros 99 | Also, added Fabrezio, Ian, and Forrest as authors of these packages. 100 | * Contributors: Forrest Rogers-Marcovitz, Ian McMahon 101 | -------------------------------------------------------------------------------- /sns_ik_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sns_ik_examples) 3 | 4 | # Verify c++11 / c++0x is valid for this compiler 5 | include(CheckCXXCompilerFlag) 6 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 7 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 8 | if(COMPILER_SUPPORTS_CXX11) 9 | set(CMAKE_CXX_FLAGS "-std=c++11 -O2") 10 | elseif(COMPILER_SUPPORTS_CXX0X) 11 | set(CMAKE_CXX_FLAGS "-std=c++0x -O2") 12 | else() 13 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++0X or C++11 support. Please choose different C++ compiler.") 14 | endif() 15 | 16 | find_package(catkin REQUIRED 17 | COMPONENTS 18 | roscpp 19 | sns_ik_lib 20 | #trac_ik_lib #Uncomment this to test against trac_ik 21 | ) 22 | 23 | find_package(Eigen3 REQUIRED) 24 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 25 | 26 | catkin_package( 27 | DEPENDS Eigen3 28 | CATKIN_DEPENDS roscpp 29 | ) 30 | 31 | include_directories( 32 | ${catkin_INCLUDE_DIRS} 33 | ${Eigen3_INCLUDE_DIRS} 34 | ) 35 | 36 | add_executable(all_ik_tests src/ik_tests.cpp) 37 | target_link_libraries(all_ik_tests ${catkin_LIBRARIES}) 38 | install(TARGETS all_ik_tests DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 39 | -------------------------------------------------------------------------------- /sns_ik_examples/README.md: -------------------------------------------------------------------------------- 1 | # README: sns_ik_examples 2 | 3 | This directory contains scripts that run some tests of the SNS-IK library using 4 | the Sawyer and Baxter robot models. 5 | 6 | ## Tutorial: how to run these scripts 7 | 8 | First you need to setup your catkin workspace. If you are unfamiliar with this 9 | process then you should refer to the ROS tutorial page: 10 | http://wiki.ros.org/catkin/Tutorials/create_a_workspace 11 | 12 | Once you set up the catkin workspace it should look something like: 13 | ```` 14 | ros_ws/ 15 | - src/ 16 | - sns_ik/ 17 | - baxter_robot/ 18 | - sawyer_robot/ 19 | ```` 20 | 21 | The `sns_ik/` directory contains this repository, cloned from: 22 | https://github.com/RethinkRobotics-opensource/sns_ik. 23 | The `baxter_robot/` director contains the open-source model of the Baxter robot, cloned from: 24 | https://github.com/RethinkRobotics/baxter_common.git. 25 | The `sawyer_robot/` directory contains the open-source model of the Sawyer robot, cloned from: 26 | https://github.com/RethinkRobotics/sawyer_robot. 27 | 28 | Once the directory structure is in place you will need to setup the catkin workspace. 29 | The commands will look something like this: 30 | ```` 31 | $ cd ros_ws 32 | $ source /opt/ros/indigo/setup.sh 33 | $ catkin_make 34 | $ source devel/setup.bash 35 | ```` 36 | 37 | Now you can run the test scripts: 38 | ```` 39 | $ roslaunch sns_ik_examples test_baxter.launch 40 | $ roslaunch sns_ik_examples test_sawyer.launch 41 | ```` 42 | 43 | ## Test architecture and parameters: 44 | 45 | The file `test_ik_solvers.launch` is the generic launch file and is called by 46 | both of the test scripts above. The launch file then passes a set of arguments 47 | into the `all_ik_tests` executable. The source code for `all_ik_tests` is in 48 | `src/ik_tests.cpp`. You can read more about ROS launch scripts here: 49 | http://wiki.ros.org/roslaunch/XML. 50 | 51 | ## How can I run the test on a different robot? 52 | 53 | One way to do this would be to create a `test_yourNewRobot.launch` file that is 54 | similar to either the Sawyer or Baxter test scripts that are in this package. 55 | -------------------------------------------------------------------------------- /sns_ik_examples/config/example_baxter_joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | left_e0: 3 | has_velocity_limits: true 4 | max_velocity: 0.75 5 | has_acceleration_limits: true 6 | max_acceleration: 2.0 7 | left_e1: 8 | has_velocity_limits: true 9 | max_velocity: 0.75 10 | has_acceleration_limits: true 11 | max_acceleration: 2.0 12 | left_s0: 13 | has_velocity_limits: true 14 | max_velocity: 0.75 15 | has_acceleration_limits: true 16 | max_acceleration: 2.0 17 | left_s1: 18 | has_velocity_limits: true 19 | max_velocity: 0.75 20 | has_acceleration_limits: true 21 | max_acceleration: 2.0 22 | left_w0: 23 | has_velocity_limits: true 24 | max_velocity: 1.0 25 | has_acceleration_limits: true 26 | max_acceleration: 2.0 27 | left_w1: 28 | has_velocity_limits: true 29 | max_velocity: 1.0 30 | has_acceleration_limits: true 31 | max_acceleration: 2.0 32 | left_w2: 33 | has_velocity_limits: true 34 | max_velocity: 1.0 35 | has_acceleration_limits: true 36 | max_acceleration: 2.0 37 | right_e0: 38 | has_velocity_limits: true 39 | max_velocity: 0.75 40 | has_acceleration_limits: true 41 | max_acceleration: 2.0 42 | right_e1: 43 | has_velocity_limits: true 44 | max_velocity: 0.75 45 | has_acceleration_limits: true 46 | max_acceleration: 2.0 47 | right_s0: 48 | has_velocity_limits: true 49 | max_velocity: 0.75 50 | has_acceleration_limits: true 51 | max_acceleration: 2.0 52 | right_s1: 53 | has_velocity_limits: true 54 | max_velocity: 0.75 55 | has_acceleration_limits: true 56 | max_acceleration: 2.0 57 | right_w0: 58 | has_velocity_limits: true 59 | max_velocity: 1.0 60 | has_acceleration_limits: true 61 | max_acceleration: 2.0 62 | right_w1: 63 | has_velocity_limits: true 64 | max_velocity: 1.0 65 | has_acceleration_limits: true 66 | max_acceleration: 2.0 67 | right_w2: 68 | has_velocity_limits: true 69 | max_velocity: 1.0 70 | has_acceleration_limits: true 71 | max_acceleration: 2.0 72 | -------------------------------------------------------------------------------- /sns_ik_examples/config/example_sawyer_joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | right_j0: 3 | has_velocity_limits: true 4 | max_velocity: 1.6 5 | has_acceleration_limits: true 6 | max_acceleration: 8.0 7 | right_j1: 8 | has_velocity_limits: true 9 | max_velocity: 1.3 10 | has_acceleration_limits: true 11 | max_acceleration: 8.0 12 | right_j2: 13 | has_velocity_limits: true 14 | max_velocity: 1.9 15 | has_acceleration_limits: true 16 | max_acceleration: 8.0 17 | right_j3: 18 | has_velocity_limits: true 19 | max_velocity: 1.9 20 | has_acceleration_limits: true 21 | max_acceleration: 8.0 22 | right_j4: 23 | has_velocity_limits: true 24 | max_velocity: 3.4 25 | has_acceleration_limits: true 26 | max_acceleration: 10.0 27 | right_j5: 28 | has_velocity_limits: true 29 | max_velocity: 3.4 30 | has_acceleration_limits: true 31 | max_acceleration: 10.0 32 | right_j6: 33 | has_velocity_limits: true 34 | max_velocity: 4.5 35 | has_acceleration_limits: true 36 | max_acceleration: 10.0 37 | -------------------------------------------------------------------------------- /sns_ik_examples/launch/test_baxter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /sns_ik_examples/launch/test_ik_solvers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /sns_ik_examples/launch/test_sawyer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /sns_ik_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sns_ik_examples 4 | 0.2.3 5 | 6 | Example programs for the Saturation in the Null Space (SNS) 7 | Inverse Kinematic Library. 8 | 9 | 10 | Fabrizio Flacco 11 | Ian McMahon 12 | Forrest Rogers-Marcovitz 13 | 14 | Rethink Robotics Inc. 15 | 16 | BSD 17 | http://ros.org/wiki/sns_ikl 18 | 19 | https://github.com/RethinkRobotics-opensource/sns_ikl 20 | 21 | 22 | https://github.com/RethinkRobotics-opensource/sns_ikl/issues 23 | 24 | 25 | catkin 26 | 27 | sns_ik_lib 28 | 29 | 30 | roscpp 31 | eigen 32 | 33 | roscpp 34 | 35 | 36 | -------------------------------------------------------------------------------- /sns_ik_kinematics_plugin/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /sns_ik_kinematics_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sns_ik_kinematics_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2016-10-25) 6 | ------------------ 7 | * No Updates 8 | 9 | 0.2.0 (2016-09-06) 10 | ------------------ 11 | * No Updates 12 | 13 | 0.1.1 (2016-04-28) 14 | --------------------------------- 15 | * No updates 16 | 17 | 0.1.0 (2016-04-22) 18 | --------------------------------- 19 | * Converts sns lib into package framework 20 | This commit lays the foundation for a collection of 21 | SNS IK packages: 22 | - sns_ik : Metapackage for SNS IK solver, plugins, examples 23 | - sns_ik_lib: SNS IK solver library 24 | - sns_ik_examples: Tests and examples for SNS IK library 25 | - sns_ik_kinematic_plugins: A package for integrating SNS IK with moveit_ros 26 | Also, added Fabrezio, Ian, and Forrest as authors of these packages. 27 | * Contributors: Ian McMahon 28 | -------------------------------------------------------------------------------- /sns_ik_kinematics_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sns_ik_kinematics_plugin) 3 | find_package(catkin REQUIRED) 4 | -------------------------------------------------------------------------------- /sns_ik_kinematics_plugin/README.md: -------------------------------------------------------------------------------- 1 | SNS IK Kinematics Plugin 2 | Package Placeholder 3 | -------------------------------------------------------------------------------- /sns_ik_kinematics_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sns_ik_kinematics_plugin 4 | 0.2.3 5 | 6 | SNS IK Kinematics Plugin for use with the MoveIT! framework. 7 | 8 | 9 | Fabrizio Flacco 10 | Ian McMahon 11 | Forrest Rogers-Marcovitz 12 | 13 | Rethink Robotics Inc. 14 | 15 | Apache 2.0 16 | http://ros.org/wiki/sns_ikl 17 | 18 | https://github.com/RethinkRobotics-opensource/sns_ikl 19 | 20 | 21 | https://github.com/RethinkRobotics-opensource/sns_ikl/issues 22 | 23 | 24 | catkin 25 | 26 | sns_ik_lib 27 | sns_ik_lib 28 | 29 | 30 | -------------------------------------------------------------------------------- /sns_ik_lib/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sns_ik_lib 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.3 (2017-10-29) 6 | ------------------ 7 | * CMakeLists Eigen cleanup 8 | In ROS Kinetic, cmake_modules is deprecated, 9 | so we will use some alternative CMakeLists 10 | strategies to find_package the Eigen 3.x library. 11 | * Fixes Eigen scalar sum warning 12 | Eigen doesn't like the fact that we're creating an Array 13 | of Bools, and then attempting to sum those booleans up. 14 | Instead, we need to cast the Array into an int, and then 15 | sum over it to store into the sum integer. 16 | Resolves https://github.com/RethinkRobotics-opensource/sns_ik/issues/56 17 | * Two small bug fixes 18 | 1) Pass vector by reference in getJointNames 19 | 2) Properly fill in matrix in pinv_forBarP 20 | * Contributors: Forrest Rogers-Marcovitz, Ian McMahon 21 | 22 | 0.2.1 (2016-10-25) 23 | ------------------ 24 | * The nullspace jacobian size was transposed when using a subset of joints 25 | * Contributors: Forrest Rogers-Marcovitz 26 | 27 | 0.2.0 (2016-09-06) 28 | ------------------ 29 | * Increase scale margin for FOSNS. Caused less unnecessary scaling with smoother motions. 30 | * Small code cleanup 31 | * Added NS velocity bias as task option 32 | * Setter for dynamic loop period 33 | * Turn on positions limits for velocity IK, but not for position IK which uses the barrier function instead. 34 | * Setters for joint Velocity and Acceleration limits 35 | * Correct nullspace projection for FSNS and FOSNS 36 | * Contributors: Forrest Rogers-Marcovitz 37 | 38 | 0.1.1 (2016-04-28) 39 | --------------------------------- 40 | * Fixed install location for sns_ik headers 41 | * Minor code cleanup / old code removal 42 | 43 | 0.1.0 (2016-04-22) 44 | --------------------------------- 45 | * Added setters for barrier function and added some comments 46 | * Barrier function implemented 47 | * Very simple limit expansion test. 48 | * Don't divid nullspace bias by timestep in position ik solver 49 | * Added nullspace gain to position and velocity solvers. 50 | * Adds correct install directory and const parameters 51 | * Turned on compile optimization -O2. Huge speed increase ~20X 52 | * Added the standard Eigen3 package find 53 | * Added a different way for CMake to detect eigen3 deps 54 | * Syntax changes for stricter build warnings 55 | * For the velocity solver in sns_ik, changed the name to CartToJntVel to minimize name confusion. 56 | * Merge with origin/master 57 | * First pass at nullspace bias tasks for position IK. Not optimized yet. 58 | * First pass at nullspace bias tasks for velocity IK. Fixed P matrix for secondary tasks in fsns and fosns. Also turned off singularity logging. 59 | * Better time comparison. 60 | * Minor performance improvements including initializing variables outside of position for-loop and not using position limits in velocity solver when solving for position. 61 | * Cleaner test output. 62 | * Fixed Interface breakage & invalid function call 63 | * Updates Position IK interface based on review comments 64 | * Adds SharedPtr Get interface for Pos & Vel solvers 65 | * Additional debug logging for fosns if LOG_ACTIVE is defined. Plus some additional code cleanup. 66 | * Additional tracking of if Cartesian velocities are scaled correctly. 67 | * Fast optimal SNS. There is still probably a bug in the code. 68 | * Errors due to using abs instead of fabs 69 | * Fast SNS algorithm 70 | * Adds the ability to set velocity solver in SNS_IK 71 | Prior to this commit, one one need to set the velocity solver manually. 72 | This introduces the public setVelocitySolveType() function for creating 73 | and changing velocity solvers from the SNS_IK class' public interface. 74 | I also created some forward declarations to prevent unnecessary linking 75 | of velocity solver headers that aren't being used in the client's code. 76 | * A few minor changes with parameters and logging. Random seed based on time. Small parameter changes. 77 | * Decreased eps and switched back to standard sns vel ik 78 | * Added testing and infra for Velocity SNS tests 79 | * Some basic improvements to the position solver so that it stays within joint limits and stops if it gets stuck 80 | * Added virtual symbol for proper inheritance 81 | * Additional debug logging. Turn off later. 82 | * Added the unused original SNS library for reference 83 | * Adds SNS_IK() class for using URDF & limits 84 | This changes adds the SNS_IK class which will construct a 85 | KDL chain from the specified robot_description parameter. 86 | Position and velocity limits can be read from the URDF, and overridden 87 | by including a standard robot_description_planning/joint_limits yaml 88 | file of lower position, upper position, max velocity and max acceleration 89 | limits. 90 | Finally, a new testing executable has been included which is based largely 91 | off of trac_ik_examples tests. This will compare sns_ik against KDL and 92 | trac_ik position solvers. 93 | * Fixed Cartesian error math which was different from Eigen 94 | * Renamend sm class 95 | * Disable setChain and getVelocityIK functions. Will redo with shared pointers later. 96 | * Fixed a number of bugs in the position ik solver, though still not converging correctly 97 | * Fixed bug in pinv_damped_P for the sub Sigma matrix 98 | * Added Optimal SNS velocity solver with scale margin 99 | * Correct inheritance 100 | * Created a class for the optimal SNS velocity IK solver 101 | * Minor change to build without warnings. 102 | * Fixed crash related to un-initialized variables 103 | * Attemp to add position ik test, but causing fault in velocity ik solver. 104 | * First attempt at position IK solver 105 | * Removed unused task elements 106 | * Better checks for input sizes 107 | * Merged in massive library formatting 108 | * Converts sns lib into package framework 109 | This commit lays the foundation for a collection of 110 | SNS IK packages: 111 | - sns_ik : Metapackage for SNS IK solver, plugins, examples 112 | - sns_ik_lib: SNS IK solver library 113 | - sns_ik_examples: Tests and examples for SNS IK library 114 | - sns_ik_kinematic_plugins: A package for integrating SNS IK with moveit_ros 115 | Also, added Fabrezio, Ian, and Forrest as authors of these packages. 116 | * Updated the source files to reflect sns_ik and lib move 117 | * Moved sns_ikl to sns_ik_lib 118 | * Contributors: Forrest Rogers-Marcovitz, Ian McMahon 119 | -------------------------------------------------------------------------------- /sns_ik_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sns_ik_lib) 3 | 4 | # Verify c++11 / c++0x is valid for this compiler 5 | include(CheckCXXCompilerFlag) 6 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 7 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 8 | if(COMPILER_SUPPORTS_CXX11) 9 | set(CMAKE_CXX_FLAGS "-std=c++11 -O2 -Wall") 10 | elseif(COMPILER_SUPPORTS_CXX0X) 11 | set(CMAKE_CXX_FLAGS "-std=c++0x -O2 -Wall") 12 | else() 13 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++0X or C++11 support. Please choose different C++ compiler.") 14 | endif() 15 | 16 | find_package(orocos_kdl REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 19 | find_package(catkin REQUIRED 20 | COMPONENTS 21 | roscpp 22 | std_msgs 23 | kdl_parser 24 | ) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS include utilities 28 | DEPENDS Eigen3 orocos_kdl 29 | CATKIN_DEPENDS roscpp std_msgs 30 | LIBRARIES sns_ik 31 | ) 32 | 33 | include_directories(utilities include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) 34 | link_directories(${orocos_kdl_LIBRARY_DIRS}) 35 | 36 | # library for public API 37 | add_library(sns_ik 38 | src/fosns_velocity_ik.cpp 39 | src/fsns_velocity_ik.cpp 40 | src/osns_sm_velocity_ik.cpp 41 | src/osns_velocity_ik.cpp 42 | src/sns_acc_ik_base.cpp 43 | src/sns_ik.cpp 44 | src/sns_ik_base.cpp 45 | src/sns_position_ik.cpp 46 | src/sns_vel_ik_base.cpp 47 | src/sns_vel_ik_base_interface.cpp 48 | src/sns_velocity_ik.cpp 49 | utilities/sns_ik_math_utils.cpp 50 | utilities/sns_linear_solver.cpp) 51 | target_link_libraries(sns_ik ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) 52 | 53 | # install the public API 54 | install(TARGETS sns_ik LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 55 | install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 56 | install(DIRECTORY utilities/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 57 | 58 | # Unit tests (google framework) 59 | if (CATKIN_ENABLE_TESTING) 60 | 61 | # library for test utilities 62 | add_library(sns_ik_test 63 | test/rng_utilities.cpp 64 | test/sawyer_model.cpp 65 | test/test_utilities.cpp) 66 | target_link_libraries(sns_ik_test sns_ik ${catkin_LIBRARIES}) 67 | 68 | # unit tests 69 | catkin_add_gtest(rng_utilities_test test/rng_utilities_test.cpp) 70 | target_link_libraries(rng_utilities_test sns_ik sns_ik_test ${catkin_LIBRARIES}) 71 | catkin_add_gtest(sns_ik_math_utils_test test/sns_ik_math_utils_test.cpp) 72 | target_link_libraries(sns_ik_math_utils_test sns_ik sns_ik_test ${catkin_LIBRARIES}) 73 | catkin_add_gtest(sns_ik_pos_test test/sns_ik_pos_test.cpp) 74 | target_link_libraries(sns_ik_pos_test sns_ik sns_ik_test ${catkin_LIBRARIES}) 75 | catkin_add_gtest(sns_ik_vel_test test/sns_ik_vel_test.cpp) 76 | target_link_libraries(sns_ik_vel_test sns_ik sns_ik_test ${catkin_LIBRARIES}) 77 | catkin_add_gtest(sns_vel_ik_base_test test/sns_vel_ik_base_test.cpp) 78 | target_link_libraries(sns_vel_ik_base_test sns_ik sns_ik_test ${catkin_LIBRARIES}) 79 | catkin_add_gtest(sns_acc_ik_base_test test/sns_acc_ik_base_test.cpp) 80 | target_link_libraries(sns_acc_ik_base_test sns_ik sns_ik_test ${catkin_LIBRARIES}) 81 | 82 | endif() 83 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/fosns_velocity_ik.hpp: -------------------------------------------------------------------------------- 1 | /*! \file fosns_velocity_ik.hpp 2 | * \brief Fast Optimal SNS velocity IK solver 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | */ 6 | /* 7 | * Copyright 2016 Rethink Robotics 8 | * 9 | * Copyright 2012-2016 Fabrizio Flacco 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | 23 | #ifndef FOSNS_IK_VELOCITY_IK 24 | #define FOSNS_IK_VELOCITY_IK 25 | 26 | #include 27 | #include 28 | 29 | #include "sns_ik/sns_velocity_ik.hpp" 30 | #include "sns_ik/fsns_velocity_ik.hpp" 31 | 32 | namespace sns_ik { 33 | 34 | class FOSNSVelocityIK : public FSNSVelocityIK { 35 | public: 36 | FOSNSVelocityIK(int dof, double loop_period); 37 | virtual ~FOSNSVelocityIK() {}; 38 | 39 | // Optimal SNS Velocity IK 40 | virtual double getJointVelocity(Eigen::VectorXd *jointVelocity, const std::vector &sot, 41 | const Eigen::VectorXd &jointConfiguration); 42 | 43 | virtual void setNumberOfTasks(int ntasks, int dof); 44 | 45 | void setScaleMargin(double scale) 46 | { scaleMargin = scale; } 47 | double getScaleMargin() 48 | { return scaleMargin; } 49 | 50 | protected: 51 | double scaleMargin; 52 | 53 | // For the FastOpt version of the SNS 54 | // TODO: should these be member variables? 55 | Eigen::MatrixXd B; //update matrix 56 | std::vector> satList; 57 | Eigen::VectorXd lagrangeMu; 58 | Eigen::VectorXd lagrangeMu1; 59 | Eigen::VectorXd lagrangeMup2w; 60 | std::forward_list::iterator it, prev_it; 61 | 62 | // Perform the SNS for a single task 63 | virtual double SNSsingle(int priority, const Eigen::VectorXd &higherPriorityJointVelocity, 64 | const Eigen::MatrixXd &higherPriorityNull, const Eigen::MatrixXd &jacobian, 65 | const Eigen::VectorXd &task, Eigen::VectorXd *jointVelocity, Eigen::MatrixXd *nullSpaceProjector); 66 | }; 67 | 68 | } // namespace sns_ik 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/fsns_velocity_ik.hpp: -------------------------------------------------------------------------------- 1 | /*! \file fsns_velocity_ik.hpp 2 | * \brief Fast SNS velocity IK solver 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | */ 6 | /* 7 | * Copyright 2016 Rethink Robotics 8 | * 9 | * Copyright 2012-2016 Fabrizio Flacco 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | 23 | #ifndef FSNS_IK_VELOCITY_IK 24 | #define FSNS_IK_VELOCITY_IK 25 | 26 | #include 27 | 28 | #include "sns_ik/sns_velocity_ik.hpp" 29 | 30 | namespace sns_ik { 31 | 32 | class FSNSVelocityIK : public SNSVelocityIK { 33 | public: 34 | FSNSVelocityIK(int dof, double loop_period); 35 | virtual ~FSNSVelocityIK() {}; 36 | 37 | // Optimal SNS Velocity IK 38 | virtual double getJointVelocity(Eigen::VectorXd *jointVelocity, const std::vector &sot, 39 | const Eigen::VectorXd &jointConfiguration); 40 | 41 | protected: 42 | // Perform the SNS for a single task 43 | virtual double SNSsingle(int priority, const Eigen::VectorXd &higherPriorityJointVelocity, 44 | const Eigen::MatrixXd &higherPriorityNull, const Eigen::MatrixXd &jacobian, 45 | const Eigen::VectorXd &task, Eigen::VectorXd *jointVelocity, Eigen::MatrixXd *nullSpaceProjector); 46 | 47 | void getTaskScalingFactor(const Eigen::ArrayXd &a, 48 | const Eigen::ArrayXd &b, 49 | const Eigen::VectorXi &S, double *scalingFactor, 50 | int *mostCriticalJoint); 51 | 52 | // TODO: Does this need to be a member variable? 53 | std::vector S; //the i-th element is zero if the i-th joint is not saturate, otherwise contains the position in B 54 | }; 55 | 56 | } // namespace sns_ik 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/osns_sm_velocity_ik.hpp: -------------------------------------------------------------------------------- 1 | /*! \file osns_sm_velocity_ik.hpp 2 | * \brief Optimal SNS, with scale margin, velocity IK solver 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | */ 6 | /* 7 | * Copyright 2016 Rethink Robotics 8 | * 9 | * Copyright 2012-2016 Fabrizio Flacco 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | 23 | #ifndef OSNS_SM_IK_VELOCITY_IK 24 | #define OSNS_SM_IK_VELOCITY_IK 25 | 26 | #include 27 | 28 | #include "sns_ik/osns_velocity_ik.hpp" 29 | 30 | namespace sns_ik { 31 | 32 | class OSNS_sm_VelocityIK : public OSNSVelocityIK { 33 | public: 34 | OSNS_sm_VelocityIK(int dof, double loop_period); 35 | virtual ~OSNS_sm_VelocityIK() {}; 36 | 37 | // Optimal SNS Velocity IK 38 | virtual double getJointVelocity(Eigen::VectorXd *jointVelocity, const std::vector &sot, 39 | const Eigen::VectorXd &jointConfiguration); 40 | 41 | void setScaleMargin(double scale) 42 | { m_scaleMargin = scale; } 43 | double getScaleMargin() 44 | { return m_scaleMargin; } 45 | 46 | protected: 47 | double m_scaleMargin; 48 | }; 49 | 50 | } // namespace sns_ik 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/osns_velocity_ik.hpp: -------------------------------------------------------------------------------- 1 | /*! \file osns_velocity_ik.hpp 2 | * \brief Optimal SNS velocity IK solver 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | */ 6 | /* 7 | * Copyright 2016 Rethink Robotics 8 | * 9 | * Copyright 2012-2016 Fabrizio Flacco 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | 23 | #ifndef OSNS_IK_VELOCITY_IK 24 | #define OSNS_IK_VELOCITY_IK 25 | 26 | #include 27 | 28 | #include "sns_ik/sns_velocity_ik.hpp" 29 | 30 | namespace sns_ik { 31 | 32 | class OSNSVelocityIK : public SNSVelocityIK { 33 | public: 34 | OSNSVelocityIK(int dof, double loop_period); 35 | virtual ~OSNSVelocityIK() {}; 36 | 37 | // Optimal SNS Velocity IK 38 | virtual double getJointVelocity(Eigen::VectorXd *jointVelocity, const std::vector &sot, 39 | const Eigen::VectorXd &jointConfiguration); 40 | 41 | protected: 42 | // Perform the SNS for a single task 43 | virtual double SNSsingle(int priority, const Eigen::VectorXd &higherPriorityJointVelocity, 44 | const Eigen::MatrixXd &higherPriorityNull, const Eigen::MatrixXd &jacobian, 45 | const Eigen::VectorXd &task, Eigen::VectorXd *jointVelocity, Eigen::MatrixXd *nullSpaceProjector); 46 | 47 | bool isOptimal(int priority, const Eigen::VectorXd& dotQ, 48 | const Eigen::MatrixXd& tildeP, Eigen::MatrixXd* W, 49 | Eigen::VectorXd* dotQn, double eps = 1e-8); 50 | }; 51 | 52 | } // namespace sns_ik 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/sns_acc_ik_base.hpp: -------------------------------------------------------------------------------- 1 | /** @file sns_acc_ik_base.hpp 2 | * 3 | * @brief The file provides the basic implementation of the SNS-IK acceleration solver 4 | * 5 | * @author Matthew Kelly 6 | * @author Andy Park 7 | * 8 | * Copyright 2018 Rethink Robotics 9 | * 10 | * Licensed under the Apache License, Version 2.0 (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License at 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #ifndef SNS_IK_LIB__SNS_ACC_IK_H_ 23 | #define SNS_IK_LIB__SNS_ACC_IK_H_ 24 | 25 | #include 26 | #include 27 | 28 | #include "sns_linear_solver.hpp" 29 | #include "sns_ik_base.hpp" 30 | 31 | namespace sns_ik { 32 | 33 | 34 | class SnsAccIkBase : SnsIkBase{ 35 | 36 | public: 37 | 38 | // Smart pointer typedefs. Note: all derived classes MUST override these smart pointers. 39 | typedef std::shared_ptr Ptr; 40 | typedef std::unique_ptr uPtr; 41 | 42 | /** 43 | * Create a default solver with nJnt joints and no bounds on joint acceleration 44 | * @param nJnt: number of joints in the robot model (columns in the jacobian) 45 | * @return: acceleration solver iff successful, nullptr otherwise 46 | */ 47 | static std::unique_ptr create(int nJnt); 48 | 49 | /** 50 | * Create a default solver with nJnt joints and infinite bounds on the joint acceleration. 51 | * @param ddqLow: lower bound on the acceleration of each joint 52 | * @param ddqUpp: upper bound on the acceleration of each joint 53 | * @return: acceleration solver iff successful, nullptr otherwise 54 | */ 55 | static std::unique_ptr create(const Eigen::ArrayXd& ddqLow, 56 | const Eigen::ArrayXd& ddqUpp); 57 | 58 | // Make sure that class is cleaned-up correctly 59 | virtual ~SnsAccIkBase() {}; 60 | 61 | /** 62 | * Solve a acceleration IK problem with no null-space bias of joint-space optimization. 63 | * 64 | * This method implements "Algorithm 1: SNS algorithm" from the paper: 65 | * "Prioritized multi-task motion control of redundant robots under hard joint constraints" 66 | * by: Fabrizio Flacco, Alessandro De Luca, Oussama Khatib 67 | * 68 | * Solve for joint acceleration ddqUpp and task scale s: 69 | * 70 | * maximize: s 71 | * subject to: 72 | * s * ddx = J * ddq + dJ*dq 73 | * 0 < s <= 1 74 | * ddqLow <= ddqUpp <= ddqUpp ( bounds set in constructor or setBounds() ) 75 | * 76 | * @param J: Jacobian matrix, mapping from joint to task space. Size = [nTask, nJoint] 77 | * @param dJdq: the product of Jacobian derivative and joint velocity. Length = nTask 78 | * @param ddx: task acceleration vector. Length = nTask 79 | * @param[out] ddq: joint acceleration solution. Length = nJoint 80 | * @param[out] taskScale: task scale. fwdKin(q, dq, ddq) = taskScale*ddx 81 | * taskScale == 1.0 --> task was feasible 82 | * taskScale < 1.0 --> task was infeasible and had to be scaled 83 | * @return: Success: the algorithm worked correctly and satisfied the problem statement 84 | * otherwise: something went wrong 85 | */ 86 | ExitCode solve(const Eigen::MatrixXd& J, const Eigen::VectorXd& dJdq, const Eigen::VectorXd& dx, 87 | Eigen::VectorXd* ddqUpp, double* taskScale); 88 | 89 | /** 90 | * Solve a acceleration IK problem with null-space bias task as the secondary goal. 91 | * 92 | * This method implements a simplified version of "Algorithm 4: SNS algorithm for multiple tasks" 93 | * where the total number of tasks is two and the secondary goal is a configuration-space velocity 94 | * task. 95 | * 96 | * Solve for joint acceleration ddqUpp and task scales (s and sCS): 97 | * 98 | * maximize: s 99 | * subject to: 100 | * s * ddx = J * ddq + dJ*dq 101 | * sCS * ddqCS = (I - pinv(J)*J) * ddq; 102 | * 0 < s <= 1 103 | * 0 < sCS <= 1 104 | * ddqLow <= ddq <= ddqUpp ( bounds set in constructor or setBounds() ) 105 | * 106 | * @param J: Jacobian matrix, mapping from joint to task space. Size = [nTask, nJoint] 107 | * @param dJdq: the product of Jacobian derivative and joint velocity. Length = nTask 108 | * @param ddx: task acceleration vector. Length = nTask 109 | * @param ddqCS: configuration space acceleration (secondary goal). Length = nJnt 110 | * @param[out] ddq: joint acceleration solution. Length = nJoint 111 | * @param[out] taskScale: task scale. fwdKin(q, dq, ddq) = taskScale*ddx 112 | * taskScale == 1.0 --> task was feasible 113 | * taskScale < 1.0 --> task was infeasible and had to be scaled 114 | * @param[out] taskScaleCS: task scale for secondary goal. 115 | * @return: Success: the algorithm worked correctly and satisfied the problem statement 116 | * otherwise: something went wrong 117 | */ 118 | ExitCode solve(const Eigen::MatrixXd& J, const Eigen::VectorXd& dJdq, const Eigen::VectorXd& ddx, 119 | const Eigen::VectorXd& ddqCS, Eigen::VectorXd* ddq, double* taskScale, double* taskScaleCS); 120 | 121 | protected: 122 | 123 | /* 124 | * protected constructor: require factory method to create an object. 125 | */ 126 | 127 | private: 128 | 129 | SnsAccIkBase(int nJnt) : SnsIkBase(nJnt) {}; 130 | 131 | }; // class SnsAccIkBase 132 | 133 | } // namespace sns_ik 134 | 135 | #endif // SNS_IK_LIB__SNS_ACC_IK_H_ 136 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/sns_ik.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Rethink Robotics 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | // Author: Ian McMahon 16 | 17 | #ifndef SNS_IK_HPP 18 | #define SNS_IK_HPP 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace sns_ik { 29 | 30 | enum VelocitySolveType { SNS, 31 | SNS_Optimal, 32 | SNS_OptimalScaleMargin, 33 | SNS_Fast, 34 | SNS_FastOptimal, 35 | SNS_Base 36 | }; 37 | 38 | 39 | /* 40 | * Convert velocity solver type to a string (for logging) 41 | * @param solverType: solve type to convert 42 | * @return: string representation of the solver type 43 | */ 44 | std::string toStr(const sns_ik::VelocitySolveType& type); 45 | 46 | // Forward declare SNS Velocity Base Class 47 | class SNSVelocityIK; 48 | class SNS_IK 49 | { 50 | public: 51 | SNS_IK(const std::string& base_link, const std::string& tip_link, 52 | const std::string& URDF_param="/robot_description", 53 | double loopPeriod=0.01, double eps=1e-5, 54 | sns_ik::VelocitySolveType type=sns_ik::SNS); 55 | 56 | SNS_IK(const KDL::Chain& chain, 57 | const KDL::JntArray& q_min, const KDL::JntArray& q_max, 58 | const KDL::JntArray& v_max, const KDL::JntArray& a_max, 59 | const std::vector& jointNames, 60 | double loopPeriod=0.01, double eps=1e-5, 61 | sns_ik::VelocitySolveType type=sns_ik::SNS); 62 | 63 | ~SNS_IK(); 64 | 65 | bool setVelocitySolveType(VelocitySolveType type); 66 | 67 | inline bool getPositionSolver(std::shared_ptr& positionSolver) { 68 | positionSolver=m_ik_pos_solver; 69 | return m_initialized; 70 | } 71 | 72 | inline bool getVelocitySolver(std::shared_ptr& velocitySolver) { 73 | velocitySolver=m_ik_vel_solver; 74 | return m_initialized; 75 | } 76 | 77 | inline bool getKDLChain(KDL::Chain& chain) { 78 | chain=m_chain; 79 | return m_initialized; 80 | } 81 | 82 | inline bool getKDLLimits(KDL::JntArray& lb, KDL::JntArray& ub, KDL::JntArray& vel, KDL::JntArray& accel) { 83 | lb=m_lower_bounds; 84 | ub=m_upper_bounds; 85 | vel=m_velocity; 86 | accel=m_acceleration; 87 | return m_initialized; 88 | } 89 | 90 | inline bool getJointNames(std::vector& jointNames) { 91 | jointNames = m_jointNames; 92 | return m_initialized; 93 | } 94 | 95 | bool setMaxJointVelocity(const KDL::JntArray& vel); 96 | 97 | bool setMaxJointAcceleration(const KDL::JntArray& accel); 98 | 99 | int CartToJnt(const KDL::JntArray &q_init, 100 | const KDL::Frame &p_in, 101 | KDL::JntArray &q_out, 102 | const KDL::Twist& bounds=KDL::Twist::Zero()) 103 | { return CartToJnt(q_init, p_in, KDL::JntArray(0), std::vector(), 104 | q_out, bounds); 105 | } 106 | 107 | // Assumes the NS bias is for all the joints in the correct order 108 | int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, 109 | const KDL::JntArray& q_bias, 110 | KDL::JntArray &q_out, 111 | const KDL::Twist& bounds=KDL::Twist::Zero()) 112 | { return CartToJnt(q_init, p_in, q_bias, m_jointNames, 113 | q_out, bounds); 114 | } 115 | 116 | int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, 117 | const KDL::JntArray& q_bias, 118 | const std::vector& biasNames, 119 | KDL::JntArray &q_out, 120 | const KDL::Twist& bounds=KDL::Twist::Zero()); 121 | 122 | int CartToJntVel(const KDL::JntArray& q_in, 123 | const KDL::Twist& v_in, 124 | KDL::JntArray& qdot_out) 125 | { return CartToJntVel(q_in, v_in, KDL::JntArray(0), std::vector(), 126 | KDL::JntArray(0), qdot_out); } 127 | 128 | // Assumes the NS bias is for all the joints in the correct order 129 | int CartToJntVel(const KDL::JntArray& q_in, 130 | const KDL::Twist& v_in, 131 | const KDL::JntArray& q_bias, 132 | KDL::JntArray& qdot_out) 133 | { return CartToJntVel(q_in, v_in, q_bias, m_jointNames, KDL::JntArray(0), qdot_out); } 134 | 135 | int CartToJntVel(const KDL::JntArray& q_in, 136 | const KDL::Twist& v_in, 137 | const KDL::JntArray& q_bias, 138 | const KDL::JntArray& q_vel_bias, 139 | KDL::JntArray& qdot_out) 140 | { return CartToJntVel(q_in, v_in, q_bias, m_jointNames, q_vel_bias, qdot_out); } 141 | 142 | int CartToJntVel(const KDL::JntArray& q_in, 143 | const KDL::Twist& v_in, 144 | const KDL::JntArray& q_bias, 145 | const std::vector& biasNames, 146 | const KDL::JntArray& q_vel_bias, 147 | KDL::JntArray& qdot_out); 148 | 149 | // Nullspace gain should be specified between 0 and 1.0 150 | double getNullspaceGain() { return m_nullspaceGain; } 151 | void setNullspaceGain(double gain) 152 | { m_nullspaceGain = std::max(std::min(gain, 1.0), 0.0); } 153 | 154 | // Set time step in seconds 155 | void setLoopPeriod(double loopPeriod); 156 | double getLoopPeriod() { return m_loopPeriod; } 157 | 158 | bool getTaskScaleFactors(std::vector& scaleFactors); 159 | 160 | private: 161 | bool m_initialized; 162 | double m_eps; 163 | double m_loopPeriod; 164 | double m_nullspaceGain; 165 | VelocitySolveType m_solvetype; 166 | KDL::Chain m_chain; 167 | KDL::JntArray m_lower_bounds, m_upper_bounds, m_velocity, m_acceleration; 168 | enum JointType { Revolute, Prismatic, Continuous }; 169 | std::vector m_types; 170 | std::vector m_jointNames; 171 | 172 | std::vector m_solutions; 173 | std::shared_ptr m_ik_vel_solver; 174 | std::shared_ptr m_ik_pos_solver; 175 | std::shared_ptr m_jacobianSolver; 176 | 177 | void initialize(); 178 | 179 | bool nullspaceBiasTask(const KDL::JntArray& q_bias, 180 | const std::vector& biasNames, 181 | Eigen::MatrixXd* jacobian, std::vector* indicies); 182 | 183 | }; 184 | } //namespace 185 | #endif 186 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/sns_position_ik.hpp: -------------------------------------------------------------------------------- 1 | /*! \file sns_position_ik.hpp 2 | * \brief Basic SNS Position IK solver 3 | * \author Forrest Rogers-Marcovitz 4 | */ 5 | /* 6 | * Copyright 2016 Rethink Robotics 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #ifndef SNS_IK_POSITION_IK 21 | #define SNS_IK_POSITION_IK 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace sns_ik { 31 | 32 | // Forward declare SNS Velocity Base Class 33 | class SNSVelocityIK; 34 | class SNSPositionIK { 35 | public: 36 | SNSPositionIK(KDL::Chain chain, std::shared_ptr velocity_ik, double eps=1e-5); 37 | ~SNSPositionIK(); 38 | 39 | int CartToJnt(const KDL::JntArray& joint_seed, 40 | const KDL::Frame& goal_pose, 41 | KDL::JntArray* return_joints, 42 | const KDL::Twist& bounds=KDL::Twist::Zero()) 43 | { return CartToJnt(joint_seed, goal_pose, KDL::JntArray(0), Eigen::MatrixXd(0,0), 44 | std::vector(0), 0.0, return_joints, bounds); } 45 | 46 | int CartToJnt(const KDL::JntArray& joint_seed, 47 | const KDL::Frame& goal_pose, 48 | const KDL::JntArray& joint_ns_bias, 49 | const Eigen::MatrixXd& ns_jacobian, 50 | const std::vector& ns_indicies, 51 | const double ns_gain, 52 | KDL::JntArray* return_joints, 53 | const KDL::Twist& bounds=KDL::Twist::Zero()); 54 | 55 | // TODO: looks like this would require the KDL solvers to be wrapped in smart pointers 56 | //void setChain(const KDL::Chain chain); 57 | KDL::Chain getChain() { return m_chain; } 58 | 59 | inline bool getVelocityIK(std::shared_ptr& velocitySolver) { 60 | velocitySolver = m_ikVelSolver; 61 | return m_ikVelSolver != NULL; 62 | } 63 | 64 | inline void setStepSize(double linearMaxStepSize, double angularMaxStepSize){ 65 | m_linearMaxStepSize = linearMaxStepSize; 66 | m_angularMaxStepSize = angularMaxStepSize; 67 | } 68 | 69 | inline void setMaxIterations(double maxIterations) { 70 | m_maxIterations = maxIterations; 71 | } 72 | 73 | void setDeltaTime(double dt) { 74 | m_dt = dt; 75 | } 76 | 77 | void setUseBarrierFunction(bool use) { 78 | m_useBarrierFunction = use; 79 | } 80 | void setBarrierInitAlpha(double alpha) { 81 | m_barrierInitAlpha = alpha; 82 | } 83 | bool setBarrierDecay(double decay) { 84 | if (decay > 0 && decay <= 1.0) { 85 | m_barrierDecay = decay; 86 | return true; 87 | } else { 88 | return false; 89 | } 90 | } 91 | 92 | private: 93 | KDL::Chain m_chain; 94 | std::shared_ptr m_ikVelSolver; 95 | KDL::ChainFkSolverPos_recursive m_positionFK; 96 | KDL::ChainJntToJacSolver m_jacobianSolver; 97 | double m_linearMaxStepSize; 98 | double m_angularMaxStepSize; 99 | double m_maxIterations; 100 | double m_eps; 101 | double m_dt; 102 | bool m_useBarrierFunction; 103 | double m_barrierInitAlpha; 104 | double m_barrierDecay; 105 | 106 | /** 107 | * @brief Calculate the position and rotation errors in base frame 108 | * @param q - joints input 109 | * @param goal - desired goal frame 110 | * @param pose - pose based of FK of q 111 | * @param errL - translation error magnitude (== trans.Norm()) 112 | * @param errR - rotational error magnitude (angle-axis representation) 113 | * @param trans - translation vector 114 | * @param rotAxis - unit rotation vector 115 | */ 116 | bool calcPoseError(const KDL::JntArray& q, 117 | const KDL::Frame& goal, 118 | KDL::Frame* pose, 119 | double* errL, 120 | double* errR, 121 | KDL::Vector* trans, 122 | KDL::Vector* rotAxis); 123 | 124 | /** 125 | * determines the rotation axis necessary to rotate from frame b1 to the 126 | * orientation of frame b2 and the vector necessary to translate the origin 127 | * of b1 to the origin of b2, and stores the result in a Twist 128 | * datastructure. The result is w.r.t. frame b1. 129 | * \param F_a_b1 frame b1 expressed with respect to some frame a. 130 | * \param F_a_b2 frame b2 expressed with respect to some frame a. 131 | * \warning The result is not a real Twist! 132 | * \warning In contrast to standard KDL diff methods, the result of 133 | * diffRelative is w.r.t. frame b1 instead of frame a. 134 | */ 135 | IMETHOD KDL::Twist diffRelative(const KDL::Frame & F_a_b1, const KDL::Frame & F_a_b2, double dt = 1) 136 | { 137 | return KDL::Twist(F_a_b1.M.Inverse() * diff(F_a_b1.p, F_a_b2.p, dt), 138 | F_a_b1.M.Inverse() * diff(F_a_b1.M, F_a_b2.M, dt)); 139 | } 140 | 141 | }; 142 | 143 | } // namespace sns_ik 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/sns_vel_ik_base.hpp: -------------------------------------------------------------------------------- 1 | /** @file sns_vel_ik_base.hpp 2 | * 3 | * @brief The file provides the basic implementation of the SNS-IK velocity solver 4 | * 5 | * @author Matthew Kelly 6 | * @author Andy Park 7 | * 8 | * Copyright 2018 Rethink Robotics 9 | * 10 | * Licensed under the Apache License, Version 2.0 (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License at 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | #ifndef SNS_IK_LIB__SNS_VEL_IK_H_ 23 | #define SNS_IK_LIB__SNS_VEL_IK_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "sns_ik_base.hpp" 30 | // #include "sns_linear_solver.hpp" 31 | 32 | namespace sns_ik { 33 | 34 | class SnsVelIkBase : public SnsIkBase{ 35 | 36 | public: 37 | 38 | // Smart pointer typedefs. Note: all derived classes MUST override these smart pointers. 39 | typedef std::shared_ptr Ptr; 40 | typedef std::unique_ptr uPtr; 41 | 42 | /** 43 | * Create a default solver with nJnt joints and no bounds on joint velocity 44 | * @param nJnt: number of joints in the robot model (columns in the jacobian) 45 | * @return: velocity solver iff successful, nullptr otherwise 46 | */ 47 | static std::unique_ptr create(int nJnt); 48 | 49 | /** 50 | * Create a default solver with constant bounds on the joint velocity 51 | * @param dqLow: lower bound on the velocity of each joint 52 | * @param dqUpp: upper bound on the velocity of each joint 53 | * @return: velocity solver iff successful, nullptr otherwise 54 | */ 55 | static std::unique_ptr create(const Eigen::ArrayXd& dqLow, 56 | const Eigen::ArrayXd& dqUpp); 57 | 58 | // Make sure that class is cleaned-up correctly 59 | virtual ~SnsVelIkBase() {}; 60 | 61 | /** 62 | * Solve a velocity IK problem with no null-space bias of joint-space optimization. 63 | * 64 | * This method implements "Algorithm 1: SNS algorithm" from the paper: 65 | * "Control of Redundant Robots Under Hard Joint Constraint: Saturation in the Null Space" 66 | * by: Fabrizio Flacco, Alessandro De Luca, Oussama Khatib 67 | * 68 | * Solve for joint velocity dq and task scale s: 69 | * 70 | * maximize: s 71 | * subject to: 72 | * s * dx = J * dq 73 | * 0 < s <= 1 74 | * dqLow <= dq <= dqUpp ( bounds set in constructor or setBounds() ) 75 | * 76 | * @param J: Jacobian matrix, mapping from joint to task space. Size = [nTask, nJoint] 77 | * @param dx: task velocity vector. Length = nTask 78 | * @param[out] dq: joint velocity solution. Length = nJoint 79 | * @param[out] taskScale: task scale. fwdKin(dq) = taskScale*dx 80 | * taskScale == 1.0 --> task was feasible 81 | * taskScale < 1.0 --> task was infeasible and had to be scaled 82 | * @return: ExitCode::Success: the algorithm worked correctly and satisfied the problem statement 83 | * otherwise: something went wrong, exit code specifics the type of problem 84 | */ 85 | ExitCode solve(const Eigen::MatrixXd& J, const Eigen::VectorXd& dx, 86 | Eigen::VectorXd* dq, double* taskScale); 87 | 88 | 89 | /** 90 | * Solve a velocity IK problem with null-space bias task as the secondary goal. 91 | * 92 | * This method implements a simplified version of "Algorithm 4: SNS algorithm for multiple tasks" 93 | * where the total number of tasks is two and the secondary goal is a configuration-space velocity 94 | * task. 95 | * 96 | * Solve for joint velocity dq and task scales (s and sCS): 97 | * 98 | * maximize: s 99 | * subject to: 100 | * s * dx = J * dq 101 | * sCS * dqCS = (I - pinv(J)*J) * dq; 102 | * 0 < s <= 1 103 | * 0 < sCS <= 1 104 | * dqLow <= dq <= dqUpp ( bounds set in constructor or setBounds() ) 105 | * 106 | * @param J: Jacobian matrix, mapping from joint to task space. Size = [nTask, nJoint] 107 | * @param dx: task space velocity vector (primary goal). Length = nTask 108 | * @param dqCS: configuration space velocity (secondary goal). Length = nJnt 109 | * @param[out] dq: joint velocity solution. Length = nJoint 110 | * @param[out] taskScale: task scale for primary goal. fwdKin(dq) = taskScale*dx 111 | * taskScale == 1.0 --> task was feasible 112 | * taskScale < 1.0 --> task was infeasible and had to be scaled 113 | * @param[out] taskScaleCS: task scale for secondary goal. 114 | * @return: ExitCode::Success: the algorithm worked correctly and satisfied the problem statement 115 | * otherwise: something went wrong, exit code specifics the type of problem 116 | */ 117 | ExitCode solve(const Eigen::MatrixXd& J, const Eigen::VectorXd& dx, const Eigen::VectorXd& dqCS, 118 | Eigen::VectorXd* dq, double* taskScale, double* taskScaleCS); 119 | 120 | protected: 121 | 122 | /* 123 | * protected constructor: require factory method to create an object. 124 | */ 125 | 126 | 127 | private: 128 | 129 | SnsVelIkBase(int nJnt) : SnsIkBase(nJnt) {}; 130 | 131 | }; // class SnsVelIkBase 132 | 133 | } // namespace sns_ik 134 | 135 | #endif // SNS_IK_LIB__SNS_VEL_IK_H_ 136 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/sns_vel_ik_base_interface.hpp: -------------------------------------------------------------------------------- 1 | /*! \file sns_velocity_base_ik.hpp 2 | * \brief Basic SNS velocity IK solver (Rethink version) 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | * \author Andy Park 6 | */ 7 | /* 8 | * Copyright 2018 Rethink Robotics 9 | * 10 | * Copyright 2012-2016 Fabrizio Flacco 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | */ 23 | 24 | #ifndef SNS_IK_VELOCITY_BASE_IK 25 | #define SNS_IK_VELOCITY_BASE_IK 26 | 27 | #include 28 | #include "sns_ik/sns_velocity_ik.hpp" 29 | #include "sns_vel_ik_base.hpp" 30 | 31 | namespace sns_ik { 32 | 33 | class SNSVelIKBaseInterface : public SNSVelocityIK { 34 | public: 35 | SNSVelIKBaseInterface(int dof, double loop_period); 36 | virtual ~SNSVelIKBaseInterface() {}; 37 | 38 | // Optimal SNS Velocity IK 39 | virtual double getJointVelocity(Eigen::VectorXd *jointVelocity, const std::vector &sot, 40 | const Eigen::VectorXd &jointConfiguration); 41 | 42 | protected: 43 | Eigen::ArrayXd dqLow; 44 | Eigen::ArrayXd dqUpp; 45 | Eigen::MatrixXd J; 46 | Eigen::VectorXd dx; 47 | Eigen::VectorXd dqCS; 48 | Eigen::VectorXd dqSol; 49 | 50 | double taskScale, taskScaleCS; 51 | 52 | SnsVelIkBase::uPtr baseIkSolver; 53 | 54 | SnsIkBase::ExitCode exitCode; 55 | }; 56 | 57 | } // namespace sns_ik 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /sns_ik_lib/include/sns_ik/sns_velocity_ik.hpp: -------------------------------------------------------------------------------- 1 | /*! \file sns_velocity_ik.hpp 2 | * \brief Basic SNS velocity IK solver 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | */ 6 | /* 7 | * Copyright 2016 Rethink Robotics 8 | * 9 | * Copyright 2012-2016 Fabrizio Flacco 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | 23 | #ifndef SNS_IK_VELOCITY_IK 24 | #define SNS_IK_VELOCITY_IK 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace sns_ik { 31 | 32 | /*! \struct Task 33 | * A desired robot task 34 | */ 35 | 36 | struct Task { 37 | Eigen::MatrixXd jacobian; //!< the task Jacobian 38 | Eigen::VectorXd desired; //!< desired velocity in task space 39 | }; 40 | 41 | static const double SHAPE_MARGIN = 0.98; 42 | 43 | class SNSVelocityIK { 44 | public: 45 | SNSVelocityIK(int dof, double loop_period); 46 | virtual ~SNSVelocityIK() {}; 47 | 48 | bool setJointsCapabilities(const Eigen::VectorXd limit_low, const Eigen::VectorXd limit_high, 49 | const Eigen::VectorXd maxVelocity, const Eigen::VectorXd maxAcceleration); 50 | bool setMaxJointVelocity(const Eigen::VectorXd maxVelocity); 51 | bool setMaxJointAcceleration(const Eigen::VectorXd maxAcceleration); 52 | virtual void setNumberOfTasks(int ntasks, int dof = -1); 53 | virtual void setNumberOfDOF(int dof); 54 | 55 | // The control loop period in seconds 56 | void setLoopPeriod(double period) { loop_period = period; } 57 | 58 | // SNS Velocity IK 59 | virtual double getJointVelocity(Eigen::VectorXd *jointVelocity, const std::vector &sot, 60 | const Eigen::VectorXd &jointConfiguration); 61 | 62 | // Standard straight inverse jacobian 63 | double getJointVelocity_STD(Eigen::VectorXd *jointVelocity, const std::vector &sot); 64 | 65 | // The standard velocity IK solver doesn't need the joint configuration, but it's here for consistancy 66 | double getJointVelocity_STD(Eigen::VectorXd *jointVelocity, const std::vector &sot, 67 | const Eigen::VectorXd &jointConfiguration) 68 | { return getJointVelocity_STD(jointVelocity, sot); } 69 | 70 | std::vector getTasksScaleFactor() 71 | { return scaleFactors; } 72 | 73 | Eigen::VectorXd getJointLimitLow() { return jointLimit_low; } 74 | Eigen::VectorXd getJointLimitHigh() { return jointLimit_high; } 75 | Eigen::VectorXd getJointVelocityMax() { return maxJointVelocity; } 76 | 77 | void usePositionLimits(bool use) { m_usePositionLimits = use; } 78 | 79 | protected: 80 | 81 | // Shape the joint velocity bound dotQmin and dotQmax 82 | void shapeJointVelocityBound(const Eigen::VectorXd &actualJointConfiguration, double margin = SHAPE_MARGIN); 83 | 84 | // Perform the SNS for a single task 85 | virtual double SNSsingle(int priority, const Eigen::VectorXd &higherPriorityJointVelocity, 86 | const Eigen::MatrixXd &higherPriorityNull, const Eigen::MatrixXd &jacobian, 87 | const Eigen::VectorXd &task, Eigen::VectorXd *jointVelocity, Eigen::MatrixXd *nullSpaceProjector); 88 | 89 | void getTaskScalingFactor(const Eigen::ArrayXd &a, 90 | const Eigen::ArrayXd &b, 91 | const Eigen::MatrixXd &W, double *scalingFactor, 92 | int *mostCriticalJoint); 93 | 94 | int n_dof; //manipulator degree of freedom 95 | int n_tasks; //number of tasks 96 | double loop_period; //needed to compute the bounds 97 | 98 | Eigen::VectorXd jointLimit_low; // low joint limits 99 | Eigen::VectorXd jointLimit_high; // high joint limit 100 | Eigen::VectorXd maxJointVelocity; // maximum joint velocity 101 | Eigen::VectorXd maxJointAcceleration; // maximum joint acceleration 102 | bool m_usePositionLimits; 103 | 104 | Eigen::ArrayXd dotQmin; // lower joint velocity bound 105 | Eigen::ArrayXd dotQmax; // higher joint velocity bound 106 | 107 | // TODO: are these needed here??? 108 | Eigen::VectorXd dotQ; // next solution (bar{\dotqv} in the paper) 109 | std::vector W; //selection matrices [here to permit a warm start] 110 | std::vector dotQopt; // next solution (bar{\dotqv} in the paper) 111 | Eigen::MatrixXd I; // identity matrix 112 | std::vector scaleFactors; 113 | 114 | std::vector nSat; //number of saturated joint 115 | }; 116 | 117 | } // namespace sns_ik 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/FindScaleFactor.m: -------------------------------------------------------------------------------- 1 | function taskScale = FindScaleFactor(low, upp, a) 2 | % taskScale = FindScaleFactor(low, upp, a) 3 | % 4 | % This function computes task scale factor from upper and lower margins and 5 | % the desired task for a single component. This function is called by all 6 | % of SNS IK algorithms. 7 | % 8 | % INPUTS: 9 | % low: lower margin 10 | % upp: upper margin 11 | % a: desired task 12 | % 13 | % OUTPUTS: 14 | % taskScale: task scale factor [0, 1] 15 | % 16 | % NOTES: 17 | % 18 | % This function is a modification of the SNS-IK algorithm 2 that is 19 | % presented in the original SNS-IK papers. It was developed by Andy Park at 20 | % Rethink Robotics in June 2018. 21 | 22 | % Copyright 2018 Rethink Robotics 23 | % 24 | % Licensed under the Apache License, Version 2.0 (the "License"); 25 | % you may not use this file except in compliance with the License. 26 | % You may obtain a copy of the License at 27 | % http://www.apache.org/licenses/LICENSE-2.0 28 | % 29 | % Unless required by applicable law or agreed to in writing, software 30 | % distributed under the License is distributed on an "AS IS" BASIS, 31 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 32 | % See the License for the specific language governing permissions and 33 | % limitations under the License. 34 | 35 | if (a < 1e10 && a > -1e10) 36 | 37 | if a < 0 && low < 0 38 | 39 | if a < low 40 | taskScale = low / a; % task is feasible with scaling 41 | else 42 | taskScale = 1.0; % task is feasible without scaling 43 | end 44 | 45 | elseif a > 0 && upp > 0 46 | 47 | if upp < a 48 | taskScale = upp / a; % task is feasible with scaling 49 | else 50 | taskScale = 1.0; % task is feasible without scaling 51 | end 52 | 53 | else 54 | taskScale = 0.0; % task is infeasible 55 | end 56 | 57 | else 58 | taskScale = 0.0; % task is infeasible 59 | end 60 | 61 | end -------------------------------------------------------------------------------- /sns_ik_lib/matlab/README.md: -------------------------------------------------------------------------------- 1 | # README -- SNS-IK matlab code 2 | 3 | This directory contains matlab implementations of a few of the basic SNS-IK 4 | algorithms. These files should be considered experimental, and are primarily 5 | included for reference and debugging purposes. 6 | 7 | ## Organization 8 | 9 | This directory contains three types of files: 10 | - `TEST_*_solvers.m` are entry-point scripts that are used to test either velocity or acceleration solvers. 11 | - `runTest_*_.m` are test functions that operate on a specific class of solvers (*eg.* velocity or acceleration). 12 | - `snsIk_*.m` and Matlab implementations for different variants of the SNS-IK solvers. 13 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/TEST_acc_ik_solvers.m: -------------------------------------------------------------------------------- 1 | % Copyright 2018 Rethink Robotics 2 | % 3 | % Licensed under the Apache License, Version 2.0 (the "License"); 4 | % you may not use this file except in compliance with the License. 5 | % You may obtain a copy of the License at 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | 14 | % This script sets up an runs a set of unit tests on the acceleration IK solvers. 15 | 16 | clc; clear; 17 | 18 | % shared test parameters; 19 | optTol = 1e-4; 20 | cstTol = 1e-6; 21 | fid = 1; 22 | nTest = 100; 23 | 24 | %% SNS-IK acceleration solver, QP implementation 25 | alpha = 1e-4; % trade-off between minimum joint speed and max task scale 26 | solver = @(dqLow, dqUpp, dx, J, dJdq)( snsIk_acc_QP(dqLow, dqUpp, dx, J, dJdq, alpha) ); 27 | result.QP = runTest_snsIk_acc(solver, nTest, optTol, cstTol, fid); 28 | 29 | %% SNS-IK solver, Rethink Robotics revised algorithm (Andy Park) 30 | result.rr = runTest_snsIk_acc(@snsIk_acc_rr, nTest, optTol, cstTol, fid); 31 | 32 | %% SNS-IK solver, Rethink Robotics revised optimal algorithm (Andy Park) 33 | result.opt = runTest_snsIk_acc(@snsIk_acc_opt, nTest, optTol, cstTol, fid); 34 | 35 | %% SNS-IK solver, Rethink Robotics revised algorithm with a secondary configuration space task (Andy Park) 36 | result.rrCS = runTest_snsIk_acc_cs(@snsIk_acc_rr_cs, nTest, optTol, cstTol, fid); 37 | fprintf(fid, 'average primary task scale factor: %.4f\n', mean(result.rrCS.sData)); 38 | fprintf(fid, 'average secondary task scale factor: %.4f\n', mean(result.rrCS.sCSData)); 39 | 40 | %% SNS-IK solver, Rethink Robotics revised algorithm with multiple tasks (Andy Park) 41 | result.rrMT = runTest_snsIk_acc_mt(@snsIk_acc_rr_mt, nTest, optTol, cstTol, fid); 42 | sDataTotal= result.rrMT.sDataTotal; 43 | sDataPrimary = zeros(nTest,1); 44 | sDataSecondary = zeros(nTest,1); 45 | numTaskData = zeros(nTest,1); 46 | numTaskFeasibleData = zeros(nTest,1); 47 | for i=1:nTest 48 | sDataTmp = sDataTotal{i}; 49 | sDataPrimary(i) = sDataTmp(1); 50 | sDataSecondary(i) = sDataTmp(2); 51 | numTaskData(i) = numel(sDataTmp); 52 | numTaskFeasibleData(i) = numel(find(sDataTmp > 0)); 53 | end 54 | fprintf(fid, 'average primary task scale factor: %.4f\n', mean(sDataPrimary)); 55 | fprintf(fid, 'average secondary task scale factor: %.4f\n', mean(sDataSecondary)); 56 | fprintf(fid, 'average number of feasible tasks: (%.4f/%.4f)\n', mean(numTaskFeasibleData), mean(numTaskData)); 57 | 58 | %% SNS-IK solver, QP implementation with multiple tasks (Andy Park) 59 | result.QPMT = runTest_snsIk_acc_mt(@snsIk_acc_QP_mt, nTest, optTol, cstTol, fid); 60 | sDataTotal= result.QPMT.sDataTotal; 61 | sDataPrimary = zeros(nTest,1); 62 | sDataSecondary = zeros(nTest,1); 63 | numTaskData = zeros(nTest,1); 64 | numTaskFeasibleData = zeros(nTest,1); 65 | for i=1:nTest 66 | sDataTmp = sDataTotal{i}; 67 | sDataPrimary(i) = sDataTmp(1); 68 | sDataSecondary(i) = sDataTmp(2); 69 | numTaskData(i) = numel(sDataTmp); 70 | numTaskFeasibleData(i) = numel(find(sDataTmp > 0)); 71 | end 72 | fprintf(fid, 'average primary task scale factor: %.4f\n', mean(sDataPrimary)); 73 | fprintf(fid, 'average secondary task scale factor: %.4f\n', mean(sDataSecondary)); 74 | fprintf(fid, 'average number of feasible tasks: (%.4f/%.4f)\n', mean(numTaskFeasibleData), mean(numTaskData)); 75 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/TEST_vel_ik_solvers.m: -------------------------------------------------------------------------------- 1 | % Copyright 2018 Rethink Robotics 2 | % 3 | % Licensed under the Apache License, Version 2.0 (the "License"); 4 | % you may not use this file except in compliance with the License. 5 | % You may obtain a copy of the License at 6 | % http://www.apache.org/licenses/LICENSE-2.0 7 | % 8 | % Unless required by applicable law or agreed to in writing, software 9 | % distributed under the License is distributed on an "AS IS" BASIS, 10 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | % See the License for the specific language governing permissions and 12 | % limitations under the License. 13 | 14 | % Run a simple unit test on the matlab implementation for the various 15 | % velocity IK solvers that are related to the original SNS-IK papers. 16 | 17 | clc; clear; 18 | 19 | % shared test parameters; 20 | optTol = 1e-4; 21 | cstTol = 1e-6; 22 | fid = 1; 23 | nTest = 100; 24 | 25 | %% SNS-IK velocity solver, QP implementation 26 | alpha = 1e-4; % trade-off between minimum joint speed and max task scale 27 | solver = @(dqLow, dqUpp, dx, J)( snsIk_vel_QP(dqLow, dqUpp, dx, J, alpha) ); 28 | result.QP = runTest_snsIk_vel(solver, nTest, optTol, cstTol, fid); 29 | 30 | %% SNS-IK solver, Rethink Robotics revised algorithm (Andy Park) 31 | result.rr = runTest_snsIk_vel(@snsIk_vel_rr, nTest, optTol, cstTol, fid); 32 | 33 | %% SNS-IK solver, Rethink Robotics revised optimal algorithm (Andy Park) 34 | result.opt = runTest_snsIk_vel(@snsIk_vel_opt, nTest, optTol, cstTol, fid); 35 | 36 | %% SNS-IK solver, Rethink Robotics revised algorithm with a secondary configuration space task (Andy Park) 37 | result.rrCS = runTest_snsIk_vel_cs(@snsIk_vel_rr_cs, nTest, optTol, cstTol, fid); 38 | fprintf(fid, 'average primary task scale factor: %.4f\n', mean(result.rrCS.sData)); 39 | fprintf(fid, 'average secondary task scale factor: %.4f\n', mean(result.rrCS.sCSData)); 40 | 41 | %% SNS-IK solver, Rethink Robotics revised algorithm for multiple tasks (Andy Park) 42 | result.rrMT = runTest_snsIk_vel_mt(@snsIk_vel_rr_mt, nTest, optTol, cstTol, fid); 43 | sDataTotal= result.rrMT.sDataTotal; 44 | sDataPrimary = zeros(nTest,1); 45 | sDataSecondary = zeros(nTest,1); 46 | numTaskData = zeros(nTest,1); 47 | numTaskFeasibleData = zeros(nTest,1); 48 | for i=1:nTest 49 | sDataTmp = sDataTotal{i}; 50 | sDataPrimary(i) = sDataTmp(1); 51 | sDataSecondary(i) = sDataTmp(2); 52 | numTaskData(i) = numel(sDataTmp); 53 | numTaskFeasibleData(i) = numel(find(sDataTmp > 0)); 54 | end 55 | fprintf(fid, 'average primary task scale factor: %.4f\n', mean(sDataPrimary)); 56 | fprintf(fid, 'average secondary task scale factor: %.4f\n', mean(sDataSecondary)); 57 | fprintf(fid, 'average number of feasible tasks: (%.4f/%.4f)\n', mean(numTaskFeasibleData), mean(numTaskData)); 58 | 59 | %% SNS-IK solver, QP implementation for multiple tasks (Andy Park) 60 | result.QPMT = runTest_snsIk_vel_mt(@snsIk_vel_QP_mt, nTest, optTol, cstTol, fid); 61 | sDataTotal= result.QPMT.sDataTotal; 62 | sDataPrimary = zeros(nTest,1); 63 | sDataSecondary = zeros(nTest,1); 64 | numTaskData = zeros(nTest,1); 65 | numTaskFeasibleData = zeros(nTest,1); 66 | for i=1:nTest 67 | sDataTmp = sDataTotal{i}; 68 | sDataPrimary(i) = sDataTmp(1); 69 | sDataSecondary(i) = sDataTmp(2); 70 | numTaskData(i) = numel(sDataTmp); 71 | numTaskFeasibleData(i) = numel(find(sDataTmp > 0)); 72 | end 73 | fprintf(fid, 'average primary task scale factor: %.4f\n', mean(sDataPrimary)); 74 | fprintf(fid, 'average secondary task scale factor: %.4f\n', mean(sDataSecondary)); 75 | fprintf(fid, 'average number of feasible tasks: (%.4f/%.4f)\n', mean(numTaskFeasibleData), mean(numTaskData)); 76 | 77 | %% SNS-IK solver, original SNS-IK algorithm 78 | % % This solver currently does not pass the test 79 | % result.basic = runTest_snsIk_vel(@snsIk_vel_basic, nTest, optTol, cstTol, fid); 80 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/runTest_snsIk_acc.m: -------------------------------------------------------------------------------- 1 | function result = runTest_snsIk_acc(solver, nTest, optTol, cstTol, fid) 2 | % result = runTest_snsIk_acc(solver, nTest, optTol, cstTol, fid) 3 | % 4 | % This function runs a unit test on a candidate acceleration IK solver 5 | % 6 | % INPUTS: 7 | % solver: acceleration Ik solver 8 | % [ddq, s, exitCode] = solver(ddqLow, ddqUpp, ddx, J) 9 | % IN: ddqLow = lower bound on joint velocity 10 | % IN: ddqUpp = upper bound on joint velocity 11 | % IN: ddx = task velocity (goal) 12 | % IN: J = task jacobian 13 | % IN: dJddq = task jacobian rate * joint angle rate 14 | % OUT: ddq = joint velocity (solution) 15 | % OUT: s = task scale 16 | % OUT: exitCode (1 == success) 17 | % nTest: how many tests to run? 18 | % optTol: optimality tolerance 19 | % cstTol: feasibility tolerance 20 | % fid: output stream for test logs (1 == command prompt) 21 | % 22 | % OUTPUTS: 23 | % result: struct with test results 24 | 25 | % Copyright 2018 Rethink Robotics 26 | % 27 | % Licensed under the Apache License, Version 2.0 (the "License"); 28 | % you may not use this file except in compliance with the License. 29 | % You may obtain a copy of the License at 30 | % http://www.apache.org/licenses/LICENSE-2.0 31 | % 32 | % Unless required by applicable law or agreed to in writing, software 33 | % distributed under the License is distributed on an "AS IS" BASIS, 34 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 35 | % See the License for the specific language governing permissions and 36 | % limitations under the License. 37 | % 38 | 39 | % run the tests 40 | nPass = 0; 41 | nFail = 0; 42 | solveTime = zeros(nTest, 1); 43 | for iTest = 1:nTest 44 | fprintf(fid, '--------------- TEST %d ---------------', iTest); 45 | 46 | % initialize the RNG seed for repeatable tests 47 | rng(iTest + 4283743); 48 | 49 | % set up the problem dimensions 50 | nJnt = randi(8); 51 | nTask = randi(nJnt); 52 | 53 | % set up the problem itself 54 | J = randn(nTask, nJnt); % jacobian 55 | dJdq = randn(nTask, 1); % jacobian rate * joint velocity 56 | ddqLow = -0.1 - rand(nJnt, 1); 57 | ddqUpp = 0.1 + rand(nJnt, 1); 58 | 59 | % compute the unscaled solution 60 | ddqTmp = ddqLow + (ddqUpp - ddqLow) .* rand(nJnt, 1); 61 | for iJnt = 1:nJnt 62 | if rand(1) < 0.2 63 | if rand(1) < 0.5 64 | ddqTmp(iJnt) = ddqLow(iJnt); 65 | else 66 | ddqTmp(iJnt) = ddqUpp(iJnt); 67 | end 68 | end 69 | end 70 | 71 | % compute the task velocity, then scale 72 | scale = 1.0; 73 | if rand(1) < 0.4; 74 | scale = 0.1 + 0.8 * rand(1); 75 | end 76 | ddxGoal = (J * ddqTmp + dJdq)/ scale; 77 | 78 | % solve the problem: 79 | testPass = true; 80 | startTime = tic; 81 | [ddq, s, exitCode] = solver(ddqLow, ddqUpp, ddxGoal, J, dJdq); 82 | solveTime(iTest) = toc(startTime); 83 | 84 | % check the solution 85 | if exitCode ~= 1 86 | fprintf(fid, '\n solver failed with exit code %d', exitCode); 87 | testPass = false; 88 | end 89 | if s < scale - optTol 90 | fprintf(fid, '\n task scale is sub-optimal! s (%.6f) < scale (%.6f)', s, scale); 91 | testPass = false; 92 | end 93 | if any(ddq > ddqUpp + cstTol) || any(ddq < ddqLow - cstTol) 94 | fprintf(fid, '\n joint limits are violated! [ddqLow, ddq, ddqUpp]'); 95 | testPass = false; 96 | end 97 | taskError = s*ddxGoal - J*ddq - dJdq; 98 | if any(abs(taskError) > cstTol) 99 | fprintf(fid, '\n task error (%.6f) exceeds tolerance!', max(abs(taskError))); 100 | testPass = false; 101 | end 102 | if testPass 103 | nPass = nPass + 1; 104 | fprintf(fid, ' --> pass\n'); 105 | else 106 | nFail = nFail + 1; 107 | fprintf(fid, '\n --> fail\n'); 108 | end 109 | end 110 | fprintf(fid, '\n'); 111 | fprintf(fid, '------------------------------------\n'); 112 | fprintf(fid, 'nPass: %d -- nFail: %d\n', nPass, nFail); 113 | fprintf(fid, '------------------------------------\n'); 114 | 115 | result.nPass = nPass; 116 | result.nFail = nFail; 117 | result.solveTime = solveTime; 118 | 119 | end 120 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/runTest_snsIk_acc_cs.m: -------------------------------------------------------------------------------- 1 | function result = runTest_snsIk_acc_cs(solver, nTest, optTol, cstTol, fid) 2 | % result = runTest_snsIk_acc(solver, nTest, optTol, cstTol, fid) 3 | % 4 | % This function runs a unit test on a candidate acceleration IK solver 5 | % that supports a configuration-space secondary objective term. 6 | % 7 | % INPUTS: 8 | % solver: acceleration Ik solver 9 | % [ddq, s, exitCode] = solver(ddqLow, ddqUpp, ddx, J) 10 | % IN: ddqLow = lower bound on joint velocity 11 | % IN: ddqUpp = upper bound on joint velocity 12 | % IN: ddx = task velocity (primary goal) 13 | % IN: ddqCS = configuration space (CS) task acceleration (secondary goal) 14 | % IN: J = task jacobian 15 | % IN: dJddq = task jacobian rate * joint angle rate 16 | % OUT: ddq = joint velocity (solution) 17 | % OUT: s = task scale 18 | % OUT: sCS = nullspace configuration space (CS) task scale 19 | % OUT: exitCode (1 == success) 20 | % nTest: how many tests to run? 21 | % optTol: optimality tolerance 22 | % cstTol: feasibility tolerance 23 | % fid: output stream for test logs (1 == command prompt) 24 | % 25 | % OUTPUTS: 26 | % result: struct with test results 27 | 28 | % Copyright 2018 Rethink Robotics 29 | % 30 | % Licensed under the Apache License, Version 2.0 (the "License"); 31 | % you may not use this file except in compliance with the License. 32 | % You may obtain a copy of the License at 33 | % http://www.apache.org/licenses/LICENSE-2.0 34 | % 35 | % Unless required by applicable law or agreed to in writing, software 36 | % distributed under the License is distributed on an "AS IS" BASIS, 37 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 38 | % See the License for the specific language governing permissions and 39 | % limitations under the License. 40 | % 41 | 42 | % run the tests 43 | nPass = 0; 44 | nFail = 0; 45 | solveTime = zeros(nTest, 1); 46 | sData = zeros(nTest,1); 47 | sCSData = zeros(nTest,1); 48 | for iTest = 1:nTest 49 | fprintf(fid, '--------------- TEST %d ---------------', iTest); 50 | 51 | % initialize the RNG seed for repeatable tests 52 | rng(iTest + 4283743); 53 | 54 | % set up the problem dimensions 55 | nJnt = randi(8); 56 | nTask = randi(nJnt); 57 | 58 | % set up the problem itself 59 | J = randn(nTask, nJnt); % jacobian 60 | dJdq = randn(nTask, 1); % jacobian rate * joint velocity 61 | ddqLow = -0.1 - rand(nJnt, 1); 62 | ddqUpp = 0.1 + rand(nJnt, 1); 63 | 64 | % compute the unscaled solution 65 | ddqTmp = ddqLow + (ddqUpp - ddqLow) .* rand(nJnt, 1); 66 | for iJnt = 1:nJnt 67 | if rand(1) < 0.2 68 | if rand(1) < 0.5 69 | ddqTmp(iJnt) = ddqLow(iJnt); 70 | else 71 | ddqTmp(iJnt) = ddqUpp(iJnt); 72 | end 73 | end 74 | end 75 | 76 | % compute the task velocity, then scale 77 | scale = 1.0; 78 | if rand(1) < 0.4 79 | scale = 0.1 + 0.8 * rand(1); 80 | end 81 | ddxGoal = (J * ddqTmp + dJdq)/ scale; 82 | 83 | % compute nullspace CS task velocity, then scale 84 | ddqCSGoal = ddqLow + (ddqUpp - ddqLow) .* rand(nJnt, 1); 85 | 86 | % solve the problem: 87 | testPass = true; 88 | startTime = tic; 89 | [ddq, s, sCS, exitCode] = solver(ddqLow, ddqUpp, ddxGoal, ddqCSGoal, J, dJdq); 90 | solveTime(iTest) = toc(startTime); 91 | sData(iTest) = s; 92 | sCSData(iTest) = sCS; 93 | 94 | % check the solution 95 | if exitCode ~= 1 96 | fprintf(fid, '\n solver failed with exit code %d', exitCode); 97 | testPass = false; 98 | end 99 | if s < scale - optTol 100 | fprintf(fid, '\n task scale is sub-optimal! s (%.6f) < scale (%.6f)', s, scale); 101 | testPass = false; 102 | end 103 | if any(ddq > ddqUpp + cstTol) || any(ddq < ddqLow - cstTol) 104 | fprintf(fid, '\n joint limits are violated! [ddqLow, ddq, ddqUpp]'); 105 | testPass = false; 106 | end 107 | taskError = s*ddxGoal - J*ddq - dJdq; 108 | if any(abs(taskError) > cstTol) 109 | fprintf(fid, '\n task error (%.6f) exceeds tolerance!', max(abs(taskError))); 110 | testPass = false; 111 | end 112 | if testPass 113 | nPass = nPass + 1; 114 | fprintf(fid, ' --> pass\n'); 115 | else 116 | nFail = nFail + 1; 117 | fprintf(fid, '\n --> fail\n'); 118 | end 119 | end 120 | fprintf(fid, '\n'); 121 | fprintf(fid, '------------------------------------\n'); 122 | fprintf(fid, 'nPass: %d -- nFail: %d\n', nPass, nFail); 123 | fprintf(fid, '------------------------------------\n'); 124 | 125 | result.nPass = nPass; 126 | result.nFail = nFail; 127 | result.solveTime = solveTime; 128 | result.sData = sData; 129 | result.sCSData = sCSData; 130 | end 131 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/runTest_snsIk_acc_mt.m: -------------------------------------------------------------------------------- 1 | function result = runTest_snsIk_acc_mt(solver, nTest, optTol, cstTol, fid) 2 | % result = runTest_snsIk_acc_mt(solver, nTest, optTol, cstTol, fid) 3 | % 4 | % This function runs a unit test on a candidate acceleration IK solver 5 | % that supports multiple task objective terms. 6 | % 7 | % INPUTS: 8 | % solver: acceleration Ik solver 9 | % [ddq, sData, exitCode] = solver(ddqLow, ddqUpp, ddxData, JData) 10 | % IN: ddqLow (nJnt x 1)= lower bound on joint acceleration 11 | % IN: ddqUpp (nJnt x 1)= upper bound on joint acceleration 12 | % IN: ddxGoalData (cell array)= task accelerations 13 | % IN: JData (cell array)= task jacobians 14 | % IN: dJdqData (cell array)= the product of task jacobian derivative and joint accelerations 15 | % OUT: ddq (nJnt x 1)= joint acceleration (solution) 16 | % OUT: sData (nTask x 1)= task scales 17 | % OUT: exitCode (1 == success) 18 | % nTest: how many tests to run? 19 | % optTol: optimality tolerance 20 | % cstTol: feasibility tolerance 21 | % fid: output stream for test logs (1 == command prompt) 22 | % 23 | % OUTPUTS: 24 | % result: struct with test results 25 | 26 | % Copyright 2018 Rethink Robotics 27 | % 28 | % Licensed under the Apache License, Version 2.0 (the "License"); 29 | % you may not use this file except in compliance with the License. 30 | % You may obtain a copy of the License at 31 | % http://www.apache.org/licenses/LICENSE-2.0 32 | % 33 | % Unless required by applicable law or agreed to in writing, software 34 | % distributed under the License is distributed on an "AS IS" BASIS, 35 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 36 | % See the License for the specific language governing permissions and 37 | % limitations under the License. 38 | 39 | % run the tests 40 | nPass = 0; 41 | nFail = 0; 42 | solveTime = zeros(nTest, 1); 43 | sDataTotal = cell(nTest,1); 44 | 45 | for iTest = 1:nTest 46 | fprintf(fid, '--------------- TEST %d ---------------', iTest); 47 | 48 | % initialize the RNG seed for repeatable tests 49 | rng(iTest + 4283743); 50 | 51 | % set up the problem dimensions 52 | nJnt = 2 + randi(7); 53 | ddqLow = -0.1 - rand(nJnt, 1); 54 | ddqUpp = 0.1 + rand(nJnt, 1); 55 | 56 | nTask = 2 + randi(4); % number of tasks 57 | nddxGoal = cell(nTask,1); 58 | scale = ones(nTask,1); 59 | JData = cell(nTask,1); 60 | dJdqData = cell(nTask,1); 61 | ddqTmp = zeros(nJnt,nTask); 62 | ddxGoal = cell(nTask,1); 63 | 64 | for iTask = 1:nTask 65 | nddxGoal{iTask} = randi(nJnt); % task space dimension 66 | 67 | % set up the problem itself 68 | JData{iTask} = randn(nddxGoal{iTask}, nJnt); % jacobian 69 | dJdqData{iTask} = randn(nddxGoal{iTask}, 1); % jacobian rate * joint velocity 70 | 71 | % compute the unscaled solution 72 | ddqTmp(:,iTask) = ddqLow + (ddqUpp - ddqLow) .* rand(nJnt, 1); 73 | for iJnt = 1:nJnt 74 | if rand(1) < 0.2 75 | if rand(1) < 0.5 76 | ddqTmp(iJnt,iTask) = ddqLow(iJnt); 77 | else 78 | ddqTmp(iJnt,iTask) = ddqUpp(iJnt); 79 | end 80 | end 81 | end 82 | 83 | % compute the task velocity, then scale 84 | if rand(1) < 0.4 85 | scale(iTask) = 0.1 + 0.8 * rand(1); 86 | end 87 | ddxGoal{iTask} = (JData{iTask} * ddqTmp(:,iTask) + dJdqData{iTask})/ scale(iTask); 88 | 89 | end 90 | 91 | % solve the problem: 92 | testPass = true; 93 | startTime = tic; 94 | [ddq, sData, exitCode] = solver(ddqLow, ddqUpp, ddxGoal, JData, dJdqData); 95 | solveTime(iTest) = toc(startTime); 96 | sDataTotal{iTest} = sData; 97 | sPrimaryTask = sDataTotal{iTest}(1); 98 | scalePrimaryTask = scale(1); 99 | 100 | % check the solution 101 | if exitCode ~= 1 102 | fprintf(fid, '\n solver failed with exit code %d', exitCode); 103 | testPass = false; 104 | end 105 | if sPrimaryTask < scalePrimaryTask - optTol 106 | fprintf(fid, '\n task scale is sub-optimal! s (%.6f) < scale (%.6f)', sPrimaryTask, scalePrimaryTask); 107 | testPass = false; 108 | end 109 | if any(ddq > ddqUpp + cstTol) || any(ddq < ddqLow - cstTol) 110 | fprintf(fid, '\n joint limits are violated! [ddqLow, ddq, ddqUpp]'); 111 | testPass = false; 112 | end 113 | taskError = sPrimaryTask*ddxGoal{1} - JData{1}*ddq - dJdqData{1}; 114 | if any(abs(taskError) > cstTol) 115 | fprintf(fid, '\n task error (%.6f) exceeds tolerance!', max(abs(taskError))); 116 | testPass = false; 117 | end 118 | if testPass 119 | nPass = nPass + 1; 120 | fprintf(fid, ' --> pass\n'); 121 | else 122 | nFail = nFail + 1; 123 | fprintf(fid, '\n --> fail\n'); 124 | end 125 | end 126 | fprintf(fid, '\n'); 127 | fprintf(fid, '------------------------------------\n'); 128 | fprintf(fid, 'nPass: %d -- nFail: %d\n', nPass, nFail); 129 | fprintf(fid, '------------------------------------\n'); 130 | 131 | result.nPass = nPass; 132 | result.nFail = nFail; 133 | result.solveTime = solveTime; 134 | result.sDataTotal = sDataTotal; 135 | end 136 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/runTest_snsIk_vel.m: -------------------------------------------------------------------------------- 1 | function result = runTest_snsIk_vel(solver, nTest, optTol, cstTol, fid) 2 | % result = runTest_snsIk_vel(solver, nTest, optTol, cstTol, fid) 3 | % 4 | % This function runs a unit test on a candidate velocity IK solver 5 | % 6 | % INPUTS: 7 | % solver: velocity Ik solver 8 | % [dq, s, exitCode] = solver(dqLow, dqUpp, dx, J) 9 | % IN: dqLow = lower bound on joint velocity 10 | % IN: dqUpp = upper bound on joint velocity 11 | % IN: dx = task velocity (goal) 12 | % IN: J = task jacobian 13 | % OUT: dq = joint velocity (solution) 14 | % OUT: s = task scale 15 | % OUT: exitCode (1 == success) 16 | % nTest: how many tests to run? 17 | % optTol: optimality tolerance 18 | % cstTol: feasibility tolerance 19 | % fid: output stream for test logs (1 == command prompt) 20 | % 21 | % OUTPUTS: 22 | % result: struct with test results 23 | % 24 | 25 | % Copyright 2018 Rethink Robotics 26 | % 27 | % Licensed under the Apache License, Version 2.0 (the "License"); 28 | % you may not use this file except in compliance with the License. 29 | % You may obtain a copy of the License at 30 | % http://www.apache.org/licenses/LICENSE-2.0 31 | % 32 | % Unless required by applicable law or agreed to in writing, software 33 | % distributed under the License is distributed on an "AS IS" BASIS, 34 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 35 | % See the License for the specific language governing permissions and 36 | % limitations under the License. 37 | % 38 | 39 | % run the tests 40 | nPass = 0; 41 | nFail = 0; 42 | solveTime = zeros(nTest, 1); 43 | for iTest = 1:nTest 44 | fprintf(fid, '--------------- TEST %d ---------------', iTest); 45 | 46 | % initialize the RNG seed for repeatable tests 47 | rng(iTest + 9872342); 48 | 49 | % set up the problem dimensions 50 | nJnt = randi(8); 51 | nTask = randi(nJnt); 52 | 53 | % set up the problem itself 54 | J = randn(nTask, nJnt); % jacobian 55 | dqLow = -0.1 - rand(nJnt, 1); 56 | dqUpp = 0.1 + rand(nJnt, 1); 57 | 58 | % compute the unscaled solution 59 | dqTmp = dqLow + (dqUpp - dqLow) .* rand(nJnt, 1); 60 | for iJnt = 1:nJnt 61 | if rand(1) < 0.2 62 | if rand(1) < 0.5 63 | dqTmp(iJnt) = dqLow(iJnt); 64 | else 65 | dqTmp(iJnt) = dqUpp(iJnt); 66 | end 67 | end 68 | end 69 | 70 | % compute the task velocity, then scale 71 | scale = 1.0; 72 | if rand(1) < 0.4; 73 | scale = 0.1 + 0.8 * rand(1); 74 | end 75 | dxGoal = J * dqTmp / scale; 76 | 77 | % solve the problem: 78 | testPass = true; 79 | startTime = tic; 80 | [dq, s, exitCode] = solver(dqLow, dqUpp, dxGoal, J); 81 | solveTime(iTest) = toc(startTime); 82 | 83 | % check the solution 84 | if exitCode ~= 1 85 | fprintf(fid, '\n solver failed with exit code %d', exitCode); 86 | testPass = false; 87 | end 88 | if s < scale - optTol 89 | fprintf(fid, '\n task scale is sub-optimal! s (%.6f) < scale (%.6f)', s, scale); 90 | testPass = false; 91 | end 92 | if any(dq > dqUpp + cstTol) || any(dq < dqLow - cstTol) 93 | fprintf(fid, '\n joint limits are violated! [dqLow, dq, dqUpp]'); 94 | testPass = false; 95 | end 96 | taskError = s*dxGoal - J*dq; 97 | if any(abs(taskError) > cstTol) 98 | fprintf(fid, '\n task error (%.6f) exceeds tolerance!', max(abs(taskError))); 99 | testPass = false; 100 | end 101 | if testPass 102 | nPass = nPass + 1; 103 | fprintf(fid, ' --> pass\n'); 104 | else 105 | nFail = nFail + 1; 106 | fprintf(fid, '\n --> fail\n'); 107 | end 108 | end 109 | fprintf(fid, '\n'); 110 | fprintf(fid, '------------------------------------\n'); 111 | fprintf(fid, 'nPass: %d -- nFail: %d\n', nPass, nFail); 112 | fprintf(fid, '------------------------------------\n'); 113 | 114 | result.nPass = nPass; 115 | result.nFail = nFail; 116 | result.solveTime = solveTime; 117 | 118 | end 119 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/runTest_snsIk_vel_cs.m: -------------------------------------------------------------------------------- 1 | function result = runTest_snsIk_vel_cs(solver, nTest, optTol, cstTol, fid) 2 | % result = runTest_snsIk_vel_cs(solver, nTest, optTol, cstTol, fid) 3 | % 4 | % This function runs a unit test on a candidate velocity IK solver 5 | % that supports a configuration-space secondary objective term. 6 | % 7 | % INPUTS: 8 | % solver: velocity Ik solver 9 | % [dq, s, exitCode] = solver(dqLow, dqUpp, dx, J) 10 | % IN: dqLow = lower bound on joint velocity 11 | % IN: dqUpp = upper bound on joint velocity 12 | % IN: dx = task velocity (primary goal) 13 | % IN: dqCS = configuration space velocity (secondary goal) 14 | % IN: J = task jacobian 15 | % OUT: dq = joint velocity (solution) 16 | % OUT: s = task scale 17 | % OUT: sCS = configuration space (CS) task scale 18 | % OUT: exitCode (1 == success) 19 | % nTest: how many tests to run? 20 | % optTol: optimality tolerance 21 | % cstTol: feasibility tolerance 22 | % fid: output stream for test logs (1 == command prompt) 23 | % 24 | % OUTPUTS: 25 | % result: struct with test results 26 | % 27 | 28 | % Copyright 2018 Rethink Robotics 29 | % 30 | % Licensed under the Apache License, Version 2.0 (the "License"); 31 | % you may not use this file except in compliance with the License. 32 | % You may obtain a copy of the License at 33 | % http://www.apache.org/licenses/LICENSE-2.0 34 | % 35 | % Unless required by applicable law or agreed to in writing, software 36 | % distributed under the License is distributed on an "AS IS" BASIS, 37 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 38 | % See the License for the specific language governing permissions and 39 | % limitations under the License. 40 | % 41 | 42 | % run the tests 43 | nPass = 0; 44 | nFail = 0; 45 | solveTime = zeros(nTest, 1); 46 | sData = zeros(nTest,1); 47 | sCSData = zeros(nTest,1); 48 | for iTest = 1:nTest 49 | fprintf(fid, '--------------- TEST %d ---------------', iTest); 50 | 51 | % initialize the RNG seed for repeatable tests 52 | rng(iTest + 9872342); 53 | 54 | % set up the problem dimensions 55 | nJnt = randi(8); 56 | nTask = randi(nJnt); 57 | 58 | % set up the problem itself 59 | J = randn(nTask, nJnt); % jacobian 60 | dqLow = -0.1 - rand(nJnt, 1); 61 | dqUpp = 0.1 + rand(nJnt, 1); 62 | 63 | % compute the unscaled solution 64 | dqTmp = dqLow + (dqUpp - dqLow) .* rand(nJnt, 1); 65 | for iJnt = 1:nJnt 66 | if rand(1) < 0.2 67 | if rand(1) < 0.5 68 | dqTmp(iJnt) = dqLow(iJnt); 69 | else 70 | dqTmp(iJnt) = dqUpp(iJnt); 71 | end 72 | end 73 | end 74 | 75 | % compute the task velocity, then scale 76 | scale = 1.0; 77 | if rand(1) < 0.4 78 | scale = 0.1 + 0.8 * rand(1); 79 | end 80 | dxGoal = J * dqTmp / scale; 81 | 82 | % compute nullspace CS task velocity, then scale 83 | dqCSGoal = dqLow + (dqUpp - dqLow) .* rand(nJnt, 1); 84 | 85 | % solve the problem: 86 | testPass = true; 87 | startTime = tic; 88 | [dq, s, sCS, exitCode] = solver(dqLow, dqUpp, dxGoal, dqCSGoal, J); 89 | solveTime(iTest) = toc(startTime); 90 | sData(iTest) = s; 91 | sCSData(iTest) = sCS; 92 | 93 | % check the solution 94 | if exitCode ~= 1 95 | fprintf(fid, '\n solver failed with exit code %d', exitCode); 96 | testPass = false; 97 | end 98 | if s < scale - optTol 99 | fprintf(fid, '\n task scale is sub-optimal! s (%.6f) < scale (%.6f)', s, scale); 100 | testPass = false; 101 | end 102 | if any(dq > dqUpp + cstTol) || any(dq < dqLow - cstTol) 103 | fprintf(fid, '\n joint limits are violated! [dqLow, dq, dqUpp]'); 104 | testPass = false; 105 | end 106 | taskError = s*dxGoal - J*dq; 107 | if any(abs(taskError) > cstTol) 108 | fprintf(fid, '\n task error (%.6f) exceeds tolerance!', max(abs(taskError))); 109 | testPass = false; 110 | end 111 | if testPass 112 | nPass = nPass + 1; 113 | fprintf(fid, ' --> pass\n'); 114 | else 115 | nFail = nFail + 1; 116 | fprintf(fid, '\n --> fail\n'); 117 | end 118 | end 119 | fprintf(fid, '\n'); 120 | fprintf(fid, '------------------------------------\n'); 121 | fprintf(fid, 'nPass: %d -- nFail: %d\n', nPass, nFail); 122 | fprintf(fid, '------------------------------------\n'); 123 | 124 | result.nPass = nPass; 125 | result.nFail = nFail; 126 | result.solveTime = solveTime; 127 | result.sData = sData; 128 | result.sCSData = sCSData; 129 | end 130 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/runTest_snsIk_vel_mt.m: -------------------------------------------------------------------------------- 1 | function result = runTest_snsIk_vel_mt(solver, nTest, optTol, cstTol, fid) 2 | % result = runTest_snsIk_vel_mt(solver, nTest, optTol, cstTol, fid) 3 | % 4 | % This function runs a unit test on a candidate velocity IK solver 5 | % that supports multiple task objective terms. 6 | % 7 | % INPUTS: 8 | % solver: velocity Ik solver 9 | % [dq, sData, exitCode] = solver(dqLow, dqUpp, dxData, JData) 10 | % IN: dqLow (nJnt x 1) = lower bound on joint velocity 11 | % IN: dqUpp (nJnt x 1) = upper bound on joint velocity 12 | % IN: dxGoalData (cell array) = task velocities 13 | % IN: JData (cell array) = task jacobians 14 | % OUT: dq (nJnt x 1) = joint velocity (solution) 15 | % OUT: sData (nTask x 1) = task scales 16 | % OUT: exitCode (1 == success) 17 | % nTest: how many tests to run? 18 | % optTol: optimality tolerance 19 | % cstTol: feasibility tolerance 20 | % fid: output stream for test logs (1 == command prompt) 21 | % 22 | % OUTPUTS: 23 | % result: struct with test results 24 | 25 | % Copyright 2018 Rethink Robotics 26 | % 27 | % Licensed under the Apache License, Version 2.0 (the "License"); 28 | % you may not use this file except in compliance with the License. 29 | % You may obtain a copy of the License at 30 | % http://www.apache.org/licenses/LICENSE-2.0 31 | % 32 | % Unless required by applicable law or agreed to in writing, software 33 | % distributed under the License is distributed on an "AS IS" BASIS, 34 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 35 | % See the License for the specific language governing permissions and 36 | % limitations under the License. 37 | 38 | % run the tests 39 | nPass = 0; 40 | nFail = 0; 41 | solveTime = zeros(nTest, 1); 42 | sDataTotal = cell(nTest,1); 43 | 44 | for iTest = 1:nTest 45 | fprintf(fid, '--------------- TEST %d ---------------', iTest); 46 | 47 | % initialize the RNG seed for repeatable tests 48 | rng(iTest + 5465454); 49 | 50 | % set up the problem dimensions 51 | nJnt = 2 + randi(8); 52 | 53 | dqLow = -0.1 - rand(nJnt, 1); 54 | dqUpp = 0.1 + rand(nJnt, 1); 55 | 56 | nTask = 2 + randi(3); % number of tasks 57 | ndxGoal = cell(nTask,1); 58 | scale = ones(nTask,1); 59 | JData = cell(nTask,1); 60 | dqTmp = zeros(nJnt,nTask); 61 | dxGoalData = cell(nTask,1); 62 | 63 | for iTask = 1:nTask 64 | ndxGoal{iTask} = randi(nJnt-2); % task space dimension 65 | 66 | % set up the problem itself 67 | JData{iTask} = randn(ndxGoal{iTask}, nJnt); % jacobian 68 | 69 | % compute the unscaled solution 70 | dqTmp(:,iTask) = dqLow + (dqUpp - dqLow) .* rand(nJnt, 1); 71 | 72 | for iJnt = 1:nJnt 73 | if rand(1) < 0.2 74 | if rand(1) < 0.5 75 | dqTmp(iJnt,iTask) = dqLow(iJnt); 76 | else 77 | dqTmp(iJnt,iTask) = dqUpp(iJnt); 78 | end 79 | end 80 | end 81 | 82 | % compute the task velocity, then scale 83 | if rand(1) < 0.4 84 | scale(iTask) = 0.1 + 0.8 * rand(1); 85 | end 86 | dxGoalData{iTask} = JData{iTask} * dqTmp(:,iTask) / scale(iTask); 87 | 88 | end 89 | 90 | % solve the problem: 91 | testPass = true; 92 | startTime = tic; 93 | [dq, sData, exitCode] = solver(dqLow, dqUpp, dxGoalData, JData); 94 | solveTime(iTest) = toc(startTime); 95 | sDataTotal{iTest} = sData; 96 | sPrimaryTask = sDataTotal{iTest}(1); 97 | scalePrimaryTask = scale(1); 98 | 99 | % check the solution 100 | if exitCode ~= 1 101 | fprintf(fid, '\n solver failed with exit code %d', exitCode); 102 | testPass = false; 103 | end 104 | if sPrimaryTask < scalePrimaryTask - optTol 105 | fprintf(fid, '\n task scale is sub-optimal! s (%.6f) < scale (%.6f)', sPrimaryTask, scalePrimaryTask); 106 | testPass = false; 107 | end 108 | if any(dq > dqUpp + cstTol) || any(dq < dqLow - cstTol) 109 | fprintf(fid, '\n joint limits are violated! [dqLow, dq, dqUpp]'); 110 | testPass = false; 111 | end 112 | taskError = sPrimaryTask*dxGoalData{1} - JData{1}*dq; 113 | if any(abs(taskError) > cstTol) 114 | fprintf(fid, '\n task error (%.6f) exceeds tolerance!', max(abs(taskError))); 115 | testPass = false; 116 | end 117 | if testPass 118 | nPass = nPass + 1; 119 | fprintf(fid, ' --> pass\n'); 120 | else 121 | nFail = nFail + 1; 122 | fprintf(fid, '\n --> fail\n'); 123 | end 124 | end 125 | fprintf(fid, '\n'); 126 | fprintf(fid, '------------------------------------\n'); 127 | fprintf(fid, 'nPass: %d -- nFail: %d\n', nPass, nFail); 128 | fprintf(fid, '------------------------------------\n'); 129 | 130 | result.nPass = nPass; 131 | result.nFail = nFail; 132 | result.solveTime = solveTime; 133 | result.sDataTotal = sDataTotal; 134 | end 135 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_acc_QP.m: -------------------------------------------------------------------------------- 1 | function [ddq, s, exitCode] = snsIk_acc_QP(ddqLow, ddqUpp, ddxGoal, J, dJdq, alpha) 2 | % [ddq, s, exitCode] = snsIk_acc_QP(ddqLow, ddqUpp, ddxGoal, J, dJdq, alpha) 3 | % 4 | % This function implements the QP version of the SNS-IK acceleration solver 5 | % 6 | % INPUTS: 7 | % ddqLow: lower limit for joint acceleration 8 | % ddqUpp: upper limit for joint acceleration 9 | % ddxGoal: task-space acceleration 10 | % J: jacoabian mapping joint space to task space 11 | % dJdq = dJ * dq 12 | % dJ = time-derivative of the task jacobian 13 | % dq = current joint velocity 14 | % alpha: trade-off between minimum-joint acceleration and maximum task scale 15 | % default: alpha = 1e-3 16 | % small alpha: favor solutions that maximize task scale 17 | % large alpha: favor minimum-joint acceleration solutions 18 | % 19 | % OUTPUTS: 20 | % ddq = joint acceleration solution with maximum task scale factor 21 | % s = task scale factor [0, 1] 22 | % 23 | % NOTES: 24 | % s * ddxGoal = J * ddq + dJ * dq; 25 | % 26 | 27 | % Copyright 2018 Rethink Robotics 28 | % 29 | % Licensed under the Apache License, Version 2.0 (the "License"); 30 | % you may not use this file except in compliance with the License. 31 | % You may obtain a copy of the License at 32 | % http://www.apache.org/licenses/LICENSE-2.0 33 | % 34 | % Unless required by applicable law or agreed to in writing, software 35 | % distributed under the License is distributed on an "AS IS" BASIS, 36 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 37 | % See the License for the specific language governing permissions and 38 | % limitations under the License. 39 | % 40 | 41 | % TODO: input validation 42 | % TODO: return optimization status 43 | 44 | if nargin < 6 45 | alpha = 1e-3; 46 | end 47 | 48 | % problem dimensions 49 | [~, nJnt] = size(J); 50 | nDecVar = nJnt + 1; % [ddq; -s] 51 | 52 | % set the objective function 53 | problem.H = blkdiag(alpha * eye(nJnt), 1.0 / alpha); 54 | problem.f = zeros(nDecVar, 1); 55 | 56 | % set the constraints: 57 | problem.lb = [ddqLow; 0]; 58 | problem.ub = [ddqUpp; 1]; 59 | problem.Aeq = [J, ddxGoal]; 60 | problem.beq = ddxGoal - dJdq; 61 | problem.Aineq = []; 62 | problem.bineq = []; 63 | 64 | % solve the problem 65 | problem.options = optimset('Display','off'); 66 | problem.solver = 'quadprog'; 67 | [zSoln, ~, exitCode] = quadprog(problem); 68 | ddq = zSoln(1:nJnt); 69 | s = 1 - zSoln(end); 70 | 71 | end 72 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_acc_QP_mt.m: -------------------------------------------------------------------------------- 1 | function [ddq, sData, exitCode] = snsIk_acc_QP_mt(ddqLow, ddqUpp, ddxGoalData, JData, dJdqData) 2 | % [ddq, sData, exitCode] = snsIk_vel_QP(ddqLow, ddqUpp, ddxGoalData, JData, dJdqData) 3 | % 4 | % This function implements the generic multi-task version of SNS-IK acceleration 5 | % solver using QP. 6 | % 7 | % INPUTS: 8 | % ddqLow (nJnt x 1): lower limit for JDataoint accelerations 9 | % ddqUpp (nJnt x 1): upper limit for JDataoint accelerations 10 | % ddxGoalData (cell array): task-space accelerations of all the tasks in the order of priorities 11 | % JData (cell array): Jacobians of all the task in corresponding order 12 | % dJdqData (cell array): dJ * dq of all the task in corresponding order 13 | % 14 | % OUTPUTS: 15 | % ddq (nJnt x 1): Joint acceleration solution with maximum task scale factor 16 | % sData (nTask x 1): task scale factors [0, 1] for all the tasks 17 | % exitCode (1 == success) 18 | % 19 | % NOTES: 20 | 21 | % Licensed under the Apache License, Version 2.0 (the "License"); 22 | % you may not use this file except in compliance with the License. 23 | % You may obtain a copy of the License at 24 | % http://www.apache.org/licenses/LICENSE-2.0 25 | % 26 | % Unless required by applicable law or agreed to in writing, software 27 | % distributed under the License is distributed on an "AS IS" BASIS, 28 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 29 | % See the License for the specific language governing permissions and 30 | % limitations under the License. 31 | 32 | % TODO: input validation 33 | % TODO: return optimization status 34 | % TODO: see if we can solve all tasks in a single QP problem 35 | 36 | % alpha: trade-off between minimum-joint velocity and maximum task scale 37 | % default: alpha = 1e-3 38 | % small alpha: favor solutions that maximize task scale 39 | % large alpha: favor minimum-joint velocity solutions 40 | alpha = 1e-3; 41 | 42 | % initialize variables 43 | numTask = size(JData,1); 44 | nJnt = numel(ddqLow); 45 | ddqData = cell(numTask,1); 46 | sData = zeros(numTask,1); 47 | resData = cell(numTask,1); 48 | 49 | % initialize problem 50 | problem.options = optimset('Display','off'); 51 | problem.solver = 'quadprog'; 52 | 53 | for iTask = 1:numTask 54 | 55 | % get i-th task jacobian 56 | Ji = JData{iTask}; 57 | nTask = size(Ji,1); 58 | 59 | % get i-th task velocity 60 | ddxGoali = ddxGoalData{iTask}; 61 | 62 | % get i-th velocity product 63 | dJdqi = dJdqData{iTask}; 64 | 65 | % equality constraint for each task (Ai*dq = bi) 66 | Ai = [Ji, ddxGoali]; 67 | bi = ddxGoali - dJdqi; 68 | 69 | % construct an augmented equality constraints for all the tasks 70 | % Aa*dq = ba 71 | if (iTask == 1) 72 | % set problem dimension 73 | nDecVar = nJnt + 1; % [dq; -s] 74 | 75 | % set the objective function 76 | problem.H = blkdiag(alpha * eye(nJnt), 1.0 / alpha); 77 | problem.f = zeros(nDecVar, 1); 78 | 79 | % set the constraints: 80 | problem.lb = [ddqLow; 0]; 81 | problem.ub = [ddqUpp; 1]; 82 | problem.Aineq = []; 83 | problem.bineq = []; 84 | 85 | problem.Aeq = Ai; 86 | problem.beq = bi; 87 | AaPrev = []; 88 | baPrev = []; 89 | else 90 | problem.Aeq = [AaPrev; Ai]; 91 | problem.beq = [baPrev; bi]; 92 | end 93 | 94 | % solve the problem 95 | [zSoln, ~, exitCode] = quadprog(problem); 96 | 97 | % if the solution becomes too small, the solution is invalid. 98 | % the current task becomes invalid 99 | if(exitCode == 1) 100 | ddqi = zSoln(1:nJnt); 101 | si = 1-zSoln(end); 102 | 103 | % residual 104 | resi = Ji*ddqi - si*ddxGoali + dJdqi; 105 | 106 | AiBar = [Ji, zeros(nTask,1)]; 107 | biBar = si*ddxGoali - dJdqi + resi; 108 | 109 | % update the previous Aa and ba with residuals 110 | AaPrev = [AaPrev; AiBar]; 111 | baPrev = [baPrev; biBar]; 112 | 113 | elseif (exitCode < 1 && iTask > 1) 114 | % if the solution for a lower priority task does not exist, 115 | % the current task becomes infeasible (i.e., si = 0) 116 | % we will keep the previous solution 117 | 118 | ddqi = ddqData{iTask-1}; 119 | si = 0; 120 | resi = Ji*ddqi - si*ddxGoali + dJdqi; 121 | exitCode = 1; 122 | 123 | else 124 | dqi = zeros(nJnt,1); 125 | si = 0; 126 | resi = Ji*dqi - si*dxGoali; 127 | end 128 | 129 | % store the solutions 130 | sData(iTask) = si; 131 | ddqData{iTask} = ddqi; 132 | resData{iTask} = resi; 133 | end 134 | 135 | ddq = ddqi; 136 | end 137 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_acc_opt.m: -------------------------------------------------------------------------------- 1 | function [ddq, s, exitCode] = snsIk_acc_opt(ddqLow, ddqUpp, ddxGoal, J, dJdq) 2 | % [ddq, s, exitCode] = snsIk_acc_opt(ddqLow, ddqUpp, ddxGoal, J, dJdq) 3 | % 4 | % This function implements the optimal version of the SNS-IK acceleration 5 | % solver for single task. 6 | % 7 | % INPUTS: 8 | % ddqLow (nJnt x 1): lower limit for joint acceleration 9 | % ddqUpp (nJnt x 1): upper limit for joint acceleration 10 | % ddxGoal (nddx x 1): task-space velocity 11 | % J (nddx x nJnt): jacoabian mapping joint space to task space 12 | % dJdq (nddx x 1) = dJ * dq 13 | % dJ = time-derivative of the task jacobian 14 | % dq = current joint velocity 15 | % 16 | % OUTPUTS: 17 | % ddq (nJnt x 1): joint acceleration solution with maximum task scale factor 18 | % s = task scale factor [0, 1] 19 | % exitCode (1 == success) 20 | % 21 | % 22 | % NOTES: 23 | % s * ddxGoal = J * ddq + dJ * dq; 24 | 25 | % Copyright 2018 Rethink Robotics 26 | % 27 | % Licensed under the Apache License, Version 2.0 (the "License"); 28 | % you may not use this file except in compliance with the License. 29 | % You may obtain a copy of the License at 30 | % http://www.apache.org/licenses/LICENSE-2.0 31 | % 32 | % Unless required by applicable law or agreed to in writing, software 33 | % distributed under the License is distributed on an "AS IS" BASIS, 34 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 35 | % See the License for the specific language governing permissions and 36 | % limitations under the License. 37 | % 38 | % TODO: input validation 39 | 40 | [nTask, nJnt] = size(J); 41 | 42 | W = eye(nJnt); 43 | ddqNull = zeros(nJnt, 1); 44 | s = 1.0; 45 | sStar = 0.0; 46 | exitCode = 1; 47 | tol = 1e-6; 48 | limitExceeded = true; 49 | while limitExceeded == true 50 | limitExceeded = false; 51 | P = eye(nJnt) - pinv(J*W)*J; 52 | ddq = pinv(J*W) * (ddxGoal - dJdq) + P * ddqNull; 53 | if any(ddq < (ddqLow - tol)) || any(ddq > (ddqUpp + tol)) 54 | limitExceeded = true; 55 | end 56 | a = pinv(J*W) * ddxGoal; 57 | b = ddq - a; 58 | 59 | marginL = ddqLow - b; 60 | marginU = ddqUpp - b; 61 | sMax = zeros(nJnt, 1); 62 | for i=1:nJnt 63 | if W(i,i) == 0 64 | sMax(i) = inf; 65 | else 66 | sMax(i) = FindScaleFactor(marginL(i), marginU(i), a(i)); 67 | % if scale factor is 1 but joint limit is violated, 68 | % set a scale factor to a small number so that the 69 | % corresponding joint will be saturated 70 | if(sMax(i) == 1 && (ddq(i) < (ddqLow(i) - tol) || ddq(i) > (ddqUpp(i) + tol))) 71 | sMax(i) = 1e-3; 72 | end 73 | end 74 | end 75 | 76 | [~, jntIdx] = min(sMax); 77 | taskScale = sMax(jntIdx); 78 | % if the current best scaled solution violates the limit, 79 | % update sStar to the latest scale factor 80 | ddqTmp = ddqNull + pinv(J*W)*(sStar * ddxGoal - dJdq - J * ddqNull); 81 | if ((taskScale > sStar) || (any(ddqTmp > ddqUpp + tol) || any(ddqTmp < ddqLow - tol))) 82 | sStar = taskScale; 83 | Wstar = W; 84 | ddqNullStar = ddqNull; 85 | end 86 | 87 | W(jntIdx, jntIdx) = 0; 88 | ddqNull(jntIdx) = min(max(ddqLow(jntIdx), ddq(jntIdx)), ddqUpp(jntIdx)); 89 | 90 | if rank(J*W) < nTask 91 | s = sStar; 92 | W = Wstar; 93 | ddqNull = ddqNullStar; 94 | ddq = ddqNull + pinv(J*W)*(s * ddxGoal - dJdq - J * ddqNull); 95 | limitExceeded = false; 96 | end 97 | 98 | % this check is added for optimal version of algorithm 99 | % do this only if at least two joints are saturated 100 | if(nJnt > 2) 101 | if(sum(diag(W)) < nJnt-2) 102 | mu = -P'*ddq; 103 | for i=1:nJnt 104 | if W(i,i) == 0 105 | if((abs(ddq(i)-ddqLow(i)) < 1e-6 && mu(i) > 0) || ... 106 | (abs(ddq(i)-ddqUpp(i)) < 1e-6 && mu(i) < 0)) 107 | % remove i-th joint from saturated joint set 108 | W(i,i) = 1; 109 | ddqNull(i) = 0; 110 | limitExceeded = true; 111 | end 112 | end 113 | end 114 | end 115 | end 116 | end 117 | 118 | end 119 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_acc_rr.m: -------------------------------------------------------------------------------- 1 | function [ddq, s, exitCode] = snsIk_acc_rr(ddqLow, ddqUpp, ddxGoal, J, dJdq) 2 | % [ddq, s, exitCode] = snsIk_acc_rr(ddqLow, ddqUpp, ddxGoal, J, dJdq) 3 | % 4 | % This function implements the basic version of the SNS-IK acceleration 5 | % solver. 6 | % 7 | % INPUTS: 8 | % ddqLow (nJnt x 1): lower limit for joint acceleration 9 | % ddqUpp (nJnt x 1): upper limit for joint acceleration 10 | % ddxGoal (ndx x 1): task-space acceleration 11 | % J (ndx x nJnt): jacoabian mapping joint space to task space 12 | % dJdq (ndx x 1) = dJ * dq 13 | % dJ = time-derivative of the task jacobian 14 | % dq = current joint velocity 15 | % 16 | % OUTPUTS: 17 | % ddq (nJnt x 1): joint acceleration solution with maximum task scale factor 18 | % s: task scale factor [0, 1] 19 | % exitCode (1 == success) 20 | % 21 | % 22 | % NOTES: 23 | % s * ddxGoal = J * ddq + dJ * dq; 24 | % 25 | % This function is a modification of the basic SNS-IK algorithm that is 26 | % presented in the original SNS-IK papers. It was developed by Andy Park at 27 | % Rethink Robotics in June 2018. 28 | 29 | % Copyright 2018 Rethink Robotics 30 | % 31 | % Licensed under the Apache License, Version 2.0 (the "License"); 32 | % you may not use this file except in compliance with the License. 33 | % You may obtain a copy of the License at 34 | % http://www.apache.org/licenses/LICENSE-2.0 35 | % 36 | % Unless required by applicable law or agreed to in writing, software 37 | % distributed under the License is distributed on an "AS IS" BASIS, 38 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 39 | % See the License for the specific language governing permissions and 40 | % limitations under the License. 41 | 42 | % TODO: input validation 43 | 44 | [nTask, nJnt] = size(J); 45 | 46 | W = eye(nJnt); 47 | ddqNull = zeros(nJnt, 1); 48 | s = 1.0; 49 | sStar = 0.0; 50 | exitCode = 1; 51 | tol = 1e-6; 52 | limitExceeded = true; 53 | while limitExceeded == true 54 | limitExceeded = false; 55 | ddq = ddqNull + pinv(J*W) * (ddxGoal -dJdq - J*ddqNull); 56 | 57 | if any(ddq < (ddqLow - tol)) || any(ddq > (ddqUpp + tol)) 58 | limitExceeded = true; 59 | end 60 | a = pinv(J*W) * ddxGoal; 61 | b = ddq - a; 62 | 63 | marginL = ddqLow - b; 64 | marginU = ddqUpp - b; 65 | sMax = zeros(nJnt, 1); 66 | for i=1:nJnt 67 | if W(i,i) == 0 68 | sMax(i) = inf; 69 | else 70 | sMax(i) = FindScaleFactor(marginL(i), marginU(i), a(i)); 71 | % if scale factor is 1 but joint limit is violated, 72 | % set a scale factor to a small number so that the 73 | % corresponding joint will be saturated 74 | if(sMax(i) == 1 && (ddq(i) < (ddqLow(i) - tol) || ddq(i) > (ddqUpp(i) + tol))) 75 | sMax(i) = 1e-3; 76 | end 77 | end 78 | end 79 | 80 | [~, jntIdx] = min(sMax); 81 | taskScale = sMax(jntIdx); 82 | % if the current best scaled solution violates the limit, 83 | % update sStar to the latest scale factor 84 | ddqTmp = ddqNull + pinv(J*W)*(sStar * ddxGoal - dJdq - J * ddqNull); 85 | if ((taskScale > sStar) || (any(ddqTmp > ddqUpp + tol) || any(ddqTmp < ddqLow - tol))) 86 | sStar = taskScale; 87 | Wstar = W; 88 | ddqNullStar = ddqNull; 89 | end 90 | 91 | W(jntIdx, jntIdx) = 0; 92 | ddqNull(jntIdx) = min(max(ddqLow(jntIdx), ddq(jntIdx)), ddqUpp(jntIdx)); 93 | 94 | if rank(J*W) < nTask 95 | s = sStar; 96 | W = Wstar; 97 | ddqNull = ddqNullStar; 98 | ddq = ddqNull + pinv(J*W)*(s * ddxGoal - dJdq - J * ddqNull); 99 | limitExceeded = false; 100 | end 101 | end 102 | end 103 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_acc_rr_cs.m: -------------------------------------------------------------------------------- 1 | function [ddq, s, sCS, exitCode] = snsIk_acc_rr_cs(ddqLow, ddqUpp, ddxGoal, ddqCS, J, dJdq) 2 | % [ddq, s, sCS, exitCode] = snsIk_acc_rr_cs(ddqLow, ddqUpp, ddxGoal, ddqCS, J, dJdq) 3 | % 4 | % This function implements a simplified multi-task version of the SNS-IK 5 | % acceleration solver that supports a secondary objective term. 6 | % In this function, the secondary task is assumed to be a desired 7 | % configuration-space acceleration. 8 | % 9 | % INPUTS: 10 | % ddqLow (nJnt x 1): lower limit for joint acceleration 11 | % ddqUpp (nJnt x 1): upper limit for joint acceleration 12 | % ddxGoal (ndx x 1): task-space acceleration (primary goal) 13 | % ddqCS (nJnt x 1) = configuration space (CS) task acceleration (secondary goal) 14 | % J(ndx x nJnt): jacoabian mapping joint space to task space 15 | % dJdq (ndx x 1) = dJ * dq 16 | % dJ = time-derivative of the task jacobian 17 | % dq = current joint velocity 18 | % 19 | % OUTPUTS: 20 | % ddq (nJnt x 1): joint acceleration solution with maximum task scale factor 21 | % s = task scale factor [0, 1] 22 | % sCS = nullspace configuration space (CS) task scale 23 | % exitCode (1 == success) 24 | % 25 | % 26 | % NOTES: 27 | % s * ddxGoal = J * ddq + dJ * dq; 28 | % sCS * ddqCS = (I - pinv(J)*J) * ddq; 29 | % 30 | % This function is a modification of the basic SNS-IK algorithm that is 31 | % presented in the original SNS-IK papers. It was developed by Andy Park at 32 | % Rethink Robotics in June 2018. 33 | 34 | % Copyright 2018 Rethink Robotics 35 | % 36 | % Licensed under the Apache License, Version 2.0 (the "License"); 37 | % you may not use this file except in compliance with the License. 38 | % You may obtain a copy of the License at 39 | % http://www.apache.org/licenses/LICENSE-2.0 40 | % 41 | % Unless required by applicable law or agreed to in writing, software 42 | % distributed under the License is distributed on an "AS IS" BASIS, 43 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 44 | % See the License for the specific language governing permissions and 45 | % limitations under the License. 46 | 47 | % TODO: input validation 48 | 49 | %% compute the solution for the primary task 50 | nJnt = size(J,2); 51 | tol = 1e-6; 52 | 53 | [ddq, s, exitCode] = snsIk_acc_rr(ddqLow, ddqUpp, ddxGoal, J, dJdq); 54 | 55 | %% compute the solution for the secondary task 56 | 57 | %-- initialization 58 | ddq1 = ddq; 59 | I = eye(nJnt); 60 | Wcs = I; 61 | 62 | %-- start of the algorithm 63 | 64 | % set Wcs in order to use only non-saturated joints from the primary task 65 | for i=1:nJnt 66 | if (abs(ddq1(i)-ddqLow(i)) < tol || abs(ddq1(i)-ddqUpp(i)) < tol) 67 | Wcs(i,i) = 0; 68 | end 69 | end 70 | 71 | % compute nullspace projection matrices 72 | % P1: nullspace projection matrix of the primary task 73 | % Pcs ensures that the projected joint velocity does not 74 | % interfere with joint saturation and the primary task 75 | % tolerance is used for numerical stability 76 | P1 = (I - pinv(J)*J); 77 | Pcs = (I - pinv((I - Wcs)*P1, tol))*P1; 78 | 79 | % validate Pcs 80 | if (norm(J*Pcs) > tol && norm((I-Wcs)*Pcs) > tol) 81 | warning('Invalid Nullspace!'); 82 | J2 = [J; (I - Wcs)]; 83 | PcsTmp = I - pinv(J2)*J2; 84 | Pcs = PcsTmp; 85 | end 86 | 87 | aCS = Pcs * ddqCS; 88 | bCS = ddq1; 89 | 90 | % compute margins 91 | marginL = ddqLow - bCS; 92 | marginU = ddqUpp - bCS; 93 | 94 | % obtain scale factor candidates 95 | sCSMax = zeros(nJnt, 1); 96 | for i=1:nJnt 97 | if (Wcs(i,i) == 0) 98 | sCSMax(i) = inf; 99 | else 100 | sCSMax(i) = FindScaleFactor(marginL(i), marginU(i), aCS(i)); 101 | end 102 | end 103 | 104 | % find the most critical joint and scale factor 105 | [~, jntIdx] = min(sCSMax); 106 | sCS = sCSMax(jntIdx); 107 | 108 | if (isinf(sCS)) 109 | % if all joints are saturated, secondary task becomes infeasible! 110 | sCS = 0; 111 | end 112 | 113 | % compute the solution with the scaleFactor 114 | ddq2 = ddq1 + sCS*Pcs*ddqCS; 115 | 116 | %-- end of algorithm 117 | 118 | ddq = ddq2; 119 | 120 | end 121 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_acc_rr_mt.m: -------------------------------------------------------------------------------- 1 | function [ddq, sData, exitCode] = snsIk_acc_rr_mt(ddqLow, ddqUpp, ddxGoalData, JData, dJdqData) 2 | % [ddq, sData, exitCode] = snsIk_acc_rr_mt(ddqLow, ddqUpp, ddxGoalDataData, JData, dJdqData) 3 | % 4 | % This function implements a generic multi-task version of the SNS-IK 5 | % acceleration solver that tries to meet multiple obJDataective terms 6 | % in a prioritized manner. 7 | % 8 | % INPUTS: 9 | % ddqLow (nJnt x 1): lower limit for joint accelerations 10 | % ddqUpp (nJnt x 1): upper limit for joint accelerations 11 | % ddxGoalData (cell array): task-space accelerations of all the tasks in the order of priorities 12 | % JData (cell array): Jacobians of all the task in corresponding order 13 | % dJdqData (cell array): dJ * dq of all the task in corresponding order 14 | % 15 | % OUTPUTS: 16 | % ddq (nJnt x 1): Joint acceleration solution with maximum task scale factor 17 | % sData (nTask x 1): task scale factors [0, 1] for all the tasks 18 | % exitCode (1 == success) 19 | % 20 | % NOTES: 21 | % 22 | % This function is a modification of the multi-task SNS-IK algorithm that is 23 | % presented in the original SNS-IK papers. It was developed by Andy Park at 24 | % Rethink Robotics in June 2018. 25 | 26 | % Copyright 2018 Rethink Robotics 27 | % 28 | % Licensed under the Apache License, Version 2.0 (the "License"); 29 | % you may not use this file except in compliance with the License. 30 | % You may obtain a copy of the License at 31 | % http://www.apache.org/licenses/LICENSE-2.0 32 | % 33 | % Unless required by applicable law or agreed to in writing, software 34 | % distributed under the License is distributed on an "AS IS" BASIS, 35 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 36 | % See the License for the specific language governing permissions and 37 | % limitations under the License. 38 | 39 | % TODO: input validation 40 | 41 | % get the number of tasks 42 | nTask = size(JData,1); 43 | WData = cell(nTask,1); 44 | sData = zeros(nTask,1); 45 | ddqData = cell(nTask,1); 46 | ddqNullData = cell(nTask,1); 47 | 48 | % initialization 49 | exitCode = 1; 50 | tol = 1e-6; 51 | 52 | nJnt = numel(ddqLow); 53 | I = eye(nJnt); 54 | Pi = I; 55 | ddqi = zeros(nJnt,1); 56 | 57 | for iTask = 1:nTask 58 | 59 | % get i-th task Jacobian 60 | Ji = JData{iTask}; 61 | nddxGoal = size(Ji,1); 62 | 63 | % get i-th task velocity 64 | ddxGoali = ddxGoalData{iTask}; 65 | 66 | % get i-th velocity product 67 | dJdqi = dJdqData{iTask}; 68 | 69 | % update variables for previous projection matrix and solution 70 | PiPrev = Pi; 71 | ddqiPrev = ddqi; 72 | 73 | % initialize variables for i-th task 74 | Wi = eye(nJnt); 75 | ddqNulli = zeros(nJnt, 1); 76 | PiBar = PiPrev; 77 | si = 1.0; 78 | siStar = 0.0; 79 | 80 | drop_correction_term = false; 81 | limitExceeded = true; 82 | cntLoop = 1; 83 | while limitExceeded == true 84 | limitExceeded = false; 85 | 86 | % compute a solution without task scale factor 87 | PiHat = (I - pinv(Ji*PiBar, tol)*Ji)*pinv((I - Wi)*PiPrev, tol); 88 | 89 | if (drop_correction_term) 90 | ddqi = ddqiPrev + pinv(Ji*PiBar, tol) * (ddxGoali - dJdqi) + ... 91 | PiHat*(ddqNulli - ddqiPrev); 92 | else 93 | ddqi = ddqiPrev + pinv(Ji*PiBar, tol) * (ddxGoali - dJdqi - Ji*ddqiPrev) + ... 94 | PiHat*(ddqNulli - ddqiPrev); 95 | end 96 | 97 | % check whether the solution violates the limits 98 | if any(ddqi < (ddqLow - tol)) || any(ddqi > (ddqUpp + tol)) 99 | limitExceeded = true; 100 | end 101 | 102 | % compute scale factor 103 | a = pinv(Ji*PiBar, tol) * ddxGoali; 104 | b = ddqi - a; 105 | 106 | marginL = ddqLow - b; 107 | marginU = ddqUpp - b; 108 | sMax = zeros(nJnt, 1); 109 | for iJnt=1:nJnt 110 | if Wi(iJnt,iJnt) == 0 111 | sMax(iJnt) = inf; 112 | else 113 | sMax(iJnt) = FindScaleFactor(marginL(iJnt), marginU(iJnt), a(iJnt)); 114 | % if scale factor is 1 but Joint limit is violated, 115 | % set a scale factor to a small number so that the 116 | % corresponding joint will be saturated 117 | if(sMax(iJnt) == 1 && (ddqi(iJnt) < (ddqLow(iJnt) - tol) || ddqi(iJnt) > (ddqUpp(iJnt) + tol))) 118 | sMax(iJnt) = tol; 119 | end 120 | end 121 | end 122 | 123 | [~, JntIdx] = min(sMax); 124 | taskScale = sMax(JntIdx); 125 | 126 | if (isinf(taskScale)) 127 | % this means that all joints are saturated, so 128 | % lower priority tasks become infeasible 129 | taskScale = 0; 130 | end 131 | 132 | % do the following if the task is feasible 133 | if (iTask == 1 || taskScale > tol) 134 | 135 | ddqiTmp = taskScale*a + b; 136 | % try to store maximum scale factor with associated variables 137 | if (taskScale > siStar || any(ddqiTmp < (ddqLow - tol)) || any(ddqiTmp > (ddqUpp + tol))) 138 | siStar = taskScale; 139 | Wistar = Wi; 140 | ddqNulliStar = ddqNulli; 141 | PiBarStar = PiBar; 142 | PiHatStar = PiHat; 143 | end 144 | 145 | % saturate the most critical joint 146 | Wi(JntIdx, JntIdx) = 0; 147 | ddqNulli(JntIdx) = min(max(ddqLow(JntIdx), ddqi(JntIdx)), ddqUpp(JntIdx)); 148 | 149 | % update projection matrices 150 | PiBar = (I - pinv((I - Wi)*PiPrev, tol))*PiPrev; 151 | PiHat = (I - pinv(Ji*PiBar, tol)*Ji)*pinv((I - Wi)*PiPrev, tol); 152 | 153 | % if rank is below task dimension, terminate the loop and output 154 | % the current best solution 155 | if (rank(Ji*PiBar) < nddxGoal) 156 | si = siStar; 157 | Wi = Wistar; 158 | ddqNulli = ddqNulliStar; 159 | PiBar = PiBarStar; 160 | PiHat = PiHatStar; 161 | 162 | if (drop_correction_term) 163 | ddqi = ddqiPrev + pinv(Ji*PiBar, tol) * (si*ddxGoali - dJdqi) + ... 164 | PiHat*(ddqNulli - ddqiPrev); 165 | 166 | limitExceeded = false; 167 | 168 | % if the solution for lower priority tasks violates the limits, ignore the solution 169 | if (iTask > 1 && (any(ddqi > ddqUpp + tol) || any(ddqi < ddqLow - tol))) 170 | si = 0; 171 | Wi = zeros(nJnt, nJnt); 172 | ddqi = ddqiPrev; 173 | end 174 | else 175 | ddqi = ddqiPrev + pinv(Ji*PiBar, tol) * (si*ddxGoali - dJdqi - Ji*ddqiPrev) + ... 176 | PiHat*(ddqNulli - ddqiPrev); 177 | 178 | % if the solution violates the limits, drop the 179 | % correction term and run it again 180 | if (any(ddqi > ddqUpp + tol) || any(ddqi < ddqLow - tol)) 181 | drop_correction_term = true; 182 | cntLoop = 0; 183 | Wi = eye(nJnt); 184 | ddqNulli = zeros(nJnt, 1); 185 | PiBar = PiPrev; 186 | si = 1.0; 187 | siStar = 0.0; 188 | else 189 | limitExceeded = false; 190 | end 191 | end 192 | end 193 | 194 | else % if the current task is infeasible 195 | si = 0; 196 | Wi = zeros(nJnt, nJnt); 197 | ddqi = ddqiPrev; 198 | limitExceeded = false; 199 | end 200 | 201 | cntLoop = cntLoop + 1; 202 | end 203 | 204 | % only if the current task is feasible, update nullspace projection 205 | if (si > 0) 206 | Pi = PiPrev - pinv(Ji*PiPrev, tol)*(Ji*PiPrev); 207 | end 208 | 209 | % store data 210 | WData{iTask} = Wi; 211 | sData(iTask) = si; 212 | ddqData{iTask} = ddqi; 213 | ddqNullData{iTask} = ddqNulli; 214 | end 215 | 216 | %-- end of algorithm 217 | ddq = ddqi; 218 | 219 | end 220 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_QP.m: -------------------------------------------------------------------------------- 1 | function [dq, s, exitCode] = snsIk_vel_QP(dqLow, dqUpp, dxGoal, J, alpha) 2 | % [dq, s, exitCode] = snsIk_vel_QP(dqLow, dqUpp, dxGoal, J, alpha) 3 | % 4 | % This function implements the basic QP version of the SNS-IK velocity 5 | % solver. 6 | % 7 | % INPUTS: 8 | % dqLow (nJnt x 1): lower limit for joint velocities 9 | % dqUpp (nJnt x 1): upper limit for joint velocities 10 | % dxGoal (nJnt x 1): task-space velocity 11 | % J (ndxGoal x nJnt): jacoabian mapping joint space to task space 12 | % alpha: trade-off between minimum-joint velocity and maximum task scale 13 | % default: alpha = 1e-3 14 | % small alpha: favor solutions that maximize task scale 15 | % large alpha: favor minimum-joint velocity solutions 16 | % 17 | % OUTPUTS: 18 | % dq (nJnt x 1): joint velocity solution with maximum task scale factor 19 | % s = task scale factor [0, 1] 20 | % 21 | % NOTES: 22 | % s * dxGoal = J * dq; 23 | 24 | % Licensed under the Apache License, Version 2.0 (the "License"); 25 | % you may not use this file except in compliance with the License. 26 | % You may obtain a copy of the License at 27 | % http://www.apache.org/licenses/LICENSE-2.0 28 | % 29 | % Unless required by applicable law or agreed to in writing, software 30 | % distributed under the License is distributed on an "AS IS" BASIS, 31 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 32 | % See the License for the specific language governing permissions and 33 | % limitations under the License. 34 | 35 | % TODO: input validation 36 | % TODO: return optimization status 37 | 38 | if nargin < 5 39 | alpha = 1e-3; 40 | end 41 | 42 | % problem dimensions 43 | [nTask, nJnt] = size(J); 44 | nDecVar = nJnt + 1; % [dq; -s] 45 | 46 | % set the objective function 47 | problem.H = blkdiag(alpha * eye(nJnt), 1.0 / alpha); 48 | problem.f = zeros(nDecVar, 1); 49 | 50 | % set the constraints: 51 | problem.lb = [dqLow; 0]; 52 | problem.ub = [dqUpp; 1]; 53 | problem.Aeq = [J, dxGoal]; 54 | problem.beq = dxGoal; 55 | problem.Aineq = []; 56 | problem.bineq = []; 57 | 58 | % solve the problem 59 | problem.options = optimset('Display','off'); 60 | problem.solver = 'quadprog'; 61 | [zSoln, ~, exitCode] = quadprog(problem); 62 | dq = zSoln(1:nJnt); 63 | s = 1-zSoln(end); 64 | 65 | end 66 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_QP_mt.m: -------------------------------------------------------------------------------- 1 | function [dq, sData, exitCode] = snsIk_vel_QP_mt(dqLow, dqUpp, dxGoalData, JData) 2 | % [dq, sData, exitCode] = snsIk_vel_QP(dqLow, dqUpp, dxGoalData, JData) 3 | % 4 | % This function implements the generic multi-task version of SNS-IK velocity 5 | % solver using QP. 6 | % 7 | % INPUTS: 8 | % dqLow (nJnt x 1): lower limit for joint velocities 9 | % dqUpp (nJnt x 1): upper limit for joint velocities 10 | % dxGoalData (cell array): task-space velocities of all the tasks in the order of priorities 11 | % JData (cell array): Jacobians of all the task in the order of priorities 12 | % 13 | % OUTPUTS: 14 | % dq (nJnt x 1): joint velocity solution with maximum task scale factor 15 | % sData (nTask x 1): task scale factors [0, 1] for all the tasks 16 | % 17 | % NOTES: 18 | 19 | % Licensed under the Apache License, Version 2.0 (the "License"); 20 | % you may not use this file except in compliance with the License. 21 | % You may obtain a copy of the License at 22 | % http://www.apache.org/licenses/LICENSE-2.0 23 | % 24 | % Unless required by applicable law or agreed to in writing, software 25 | % distributed under the License is distributed on an "AS IS" BASIS, 26 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 27 | % See the License for the specific language governing permissions and 28 | % limitations under the License. 29 | 30 | % TODO: input validation 31 | % TODO: return optimization status 32 | % TODO: see if we can solve all tasks in a single QP problem 33 | 34 | % alpha: trade-off between minimum-joint velocity and maximum task scale 35 | % default: alpha = 1e-3 36 | % small alpha: favor solutions that maximize task scale 37 | % large alpha: favor minimum-joint velocity solutions 38 | alpha = 1e-3; 39 | 40 | % initialize variables 41 | numTask = size(JData,1); 42 | nJnt = numel(dqLow); 43 | dqData = cell(numTask,1); 44 | sData = zeros(numTask,1); 45 | resData = cell(numTask,1); 46 | 47 | % initialize problem 48 | problem.options = optimset('Display','off'); 49 | problem.solver = 'quadprog'; 50 | 51 | for iTask = 1:numTask 52 | 53 | % get i-th task jacobian 54 | Ji = JData{iTask}; 55 | nTask = size(Ji,1); 56 | 57 | % get i-th task velocity 58 | dxGoali = dxGoalData{iTask}; 59 | 60 | % equality constraint for each task (Ai*dq = bi) 61 | Ai = [Ji, dxGoali]; 62 | bi = dxGoali; 63 | 64 | % construct an augmented equality constraints for all the tasks 65 | % Aa*dq = ba 66 | if (iTask == 1) 67 | 68 | % set problem dimension 69 | nDecVar = nJnt + 1; % [dq; -s] 70 | 71 | % set the objective function 72 | problem.H = blkdiag(alpha * eye(nJnt), 1.0 / alpha); 73 | problem.f = zeros(nDecVar, 1); 74 | 75 | % set the constraints: 76 | problem.lb = [dqLow; 0]; 77 | problem.ub = [dqUpp; 1]; 78 | problem.Aineq = []; 79 | problem.bineq = []; 80 | 81 | problem.Aeq = Ai; 82 | problem.beq = bi; 83 | AaPrev = []; 84 | baPrev = []; 85 | else 86 | problem.Aeq = [AaPrev; Ai]; 87 | problem.beq = [baPrev; bi]; 88 | end 89 | 90 | % solve the problem 91 | [zSoln, ~, exitCode] = quadprog(problem); 92 | 93 | if (exitCode == 1) 94 | dqi = zSoln(1:nJnt); 95 | si = 1-zSoln(end); 96 | 97 | % residual 98 | resi = Ji*dqi - si*dxGoali; 99 | 100 | AiBar = [Ji, zeros(nTask,1)]; 101 | biBar = si*dxGoali + resi; 102 | 103 | % update the previous Aa and ba with residuals 104 | AaPrev = [AaPrev; AiBar]; 105 | baPrev = [baPrev; biBar]; 106 | 107 | elseif (exitCode < 1 && iTask > 1) 108 | % if the solution for a lower priority task does not exist, 109 | % the current task becomes infeasible (i.e., si = 0) 110 | % we will keep the previous solution 111 | dqi = dqData{iTask-1}; 112 | si = 0; 113 | resi = Ji*dqi - si*dxGoali; 114 | 115 | exitCode = 1; 116 | 117 | else 118 | dqi = zeros(nJnt,1); 119 | si = 0; 120 | resi = Ji*dqi - si*dxGoali; 121 | end 122 | 123 | % store the solutions 124 | sData(iTask) = si; 125 | dqData{iTask} = dqi; 126 | resData{iTask} = resi; 127 | end 128 | 129 | dq = dqi; 130 | end 131 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_basic.m: -------------------------------------------------------------------------------- 1 | function [dq, s, exitCode] = snsIk_vel_basic(dqLow, dqUpp, dx, J) 2 | % [dq, s, exitCode] = snsIk_vel_basic(dqLow, dqUpp, dx, J) 3 | % 4 | % This function implements the basic version of the SNS-IK velocity 5 | % solver. 6 | % 7 | % INPUTS: 8 | % dqLow: lower limit for joint velocities 9 | % dqUpp: upper limit for joint velocities 10 | % dx: task-space velocity 11 | % J: jacoabian mapping joint space to task space 12 | % 13 | % OUTPUTS: 14 | % dq = joint velocity solution with maximum task scale factor 15 | % s = task scale factor [0, 1] 16 | % exitCode (1 == success) 17 | % 18 | % 19 | % NOTES: 20 | % s * dxGoal = J * dq; 21 | % 22 | % --> This implementation is as close to the standard SNS-IK algorithm as 23 | % is possible, outlined in the main SNS-IK paper. 24 | % 25 | 26 | % Copyright 2018 Rethink Robotics 27 | % 28 | % Licensed under the Apache License, Version 2.0 (the "License"); 29 | % you may not use this file except in compliance with the License. 30 | % You may obtain a copy of the License at 31 | % http://www.apache.org/licenses/LICENSE-2.0 32 | % 33 | % Unless required by applicable law or agreed to in writing, software 34 | % distributed under the License is distributed on an "AS IS" BASIS, 35 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 36 | % See the License for the specific language governing permissions and 37 | % limitations under the License. 38 | % 39 | 40 | % TODO: input validation 41 | % TODO: return optimization status 42 | 43 | % This implementation is not reliable, and in many cases will fail to 44 | % compute the correct solution. It is unclear whether this is a bug in 45 | % this implementation, or a bug in the algorithm that is described in the 46 | % paper. See snsIk_vel_rr() for a modified version of this algorithm that 47 | % does pass all of the unit tests. 48 | warning('This implementation does not work!'); 49 | 50 | [nTask, nJnt] = size(J); 51 | 52 | W = eye(nJnt); 53 | dqNull = zeros(nJnt, 1); 54 | s = 1.0; 55 | sStar = 0.0; 56 | maxIter = 5 * nJnt; % over-estimate of how many iterations to expect 57 | for iter = 1:maxIter 58 | limitExceeded = false; 59 | dq = dqNull + pinv(J*W) * (dx - J*dqNull); 60 | if any(dq < dqLow) || any(dq > dqUpp) 61 | limitExceeded = true; 62 | end 63 | a = pinv(J*W) * dx; 64 | b = dq - a; 65 | [taskScale, jntIdx] = getTaskScalingFactor(a, b, dqLow, dqUpp); 66 | if taskScale > sStar 67 | sStar = taskScale; 68 | Wstar = W; 69 | dqNullStar = dqNull; 70 | end 71 | W(jntIdx, jntIdx) = 0; 72 | dq(jntIdx) = min(max(dqLow(jntIdx), dq(jntIdx)), dqUpp(jntIdx)); 73 | if rank(J*W) < nTask 74 | s = sStar; 75 | W = Wstar; 76 | dqNull = dqNullStar; 77 | dq = dqNull + pinv(J*W)*(s * dx - J * dqNull); 78 | % limitExceeded = false; % unreachable 79 | break; 80 | end 81 | if limitExceeded 82 | break; 83 | end 84 | end 85 | 86 | % TODO: better exit code 87 | if iter < maxIter 88 | exitCode = 1; 89 | else 90 | exitCode = -1; 91 | end 92 | 93 | end 94 | 95 | %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~% 96 | 97 | function [taskScale, jntIdx] = getTaskScalingFactor(a, b, dqLow, dqUpp) 98 | 99 | % TODO: documentation 100 | 101 | sMin = (dqLow - b) ./ a; 102 | sMax = (dqUpp - b) ./ a; 103 | for i=1:length(sMin) 104 | if sMin(i) > sMax(i) 105 | tmp = sMin(i); 106 | sMin(i) = sMax(i); 107 | sMax(i) = tmp; 108 | end 109 | end 110 | 111 | [sMaxVal, sMaxIdx] = min(sMax); 112 | sMinVal = max(sMin); 113 | if sMinVal > sMaxVal || sMaxVal < 0.0 || sMinVal > 1.0 114 | taskScale = 0.0; 115 | warning('Infeasible solution!'); 116 | else 117 | taskScale = sMaxVal; 118 | jntIdx = sMaxIdx; 119 | end 120 | 121 | end 122 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_opt.m: -------------------------------------------------------------------------------- 1 | function [dq, s, exitCode] = snsIk_vel_opt(dqLow, dqUpp, dxGoal, J) 2 | % [dq, s, exitCode] = snsIk_vel_opt(dqLow, dqUpp, dxGoal, J) 3 | % 4 | % This function implements the optimal version of the SNS-IK velocity 5 | % solver for single task. 6 | % 7 | % INPUTS: 8 | % dqLow (nJnt x 1): lower limit for joint velocities 9 | % dqUpp (nJnt x 1): upper limit for joint velocities 10 | % dxGoal (ndx x 1): task-space velocity 11 | % J (ndx x nJnt): jacoabian mapping joint space to task space 12 | % 13 | % OUTPUTS: 14 | % dq (nJnt x 1): joint velocity solution with maximum task scale factor 15 | % s = task scale factor [0, 1] 16 | % exitCode (1 == success) 17 | % 18 | % 19 | % NOTES: 20 | % s * dxGoal = J * dq; 21 | 22 | % Copyright 2018 Rethink Robotics 23 | % 24 | % Licensed under the Apache License, Version 2.0 (the "License"); 25 | % you may not use this file except in compliance with the License. 26 | % You may obtain a copy of the License at 27 | % http://www.apache.org/licenses/LICENSE-2.0 28 | % 29 | % Unless required by applicable law or agreed to in writing, software 30 | % distributed under the License is distributed on an "AS IS" BASIS, 31 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 32 | % See the License for the specific language governing permissions and 33 | % limitations under the License. 34 | 35 | % TODO: input validation 36 | 37 | [nTask, nJnt] = size(J); 38 | 39 | W = eye(nJnt); 40 | dqNull = zeros(nJnt, 1); 41 | s = 1.0; 42 | sStar = 0.0; 43 | exitCode = 1; 44 | tol = 1e-6; 45 | limitExceeded = true; 46 | while limitExceeded == true 47 | limitExceeded = false; 48 | P = eye(nJnt) - pinv(J*W)*J; 49 | dq = pinv(J*W) * dxGoal + P*dqNull; 50 | if any(dq < (dqLow - tol)) || any(dq > (dqUpp + tol)) 51 | limitExceeded = true; 52 | end 53 | a = pinv(J*W) * dxGoal; 54 | b = dq - a; 55 | 56 | marginL = dqLow - b; 57 | marginU = dqUpp - b; 58 | sMax = zeros(nJnt, 1); 59 | for i=1:nJnt 60 | if W(i,i) == 0 61 | sMax(i) = inf; 62 | else 63 | sMax(i) = FindScaleFactor(marginL(i), marginU(i), a(i)); 64 | end 65 | end 66 | 67 | [~, jntIdx] = min(sMax); 68 | taskScale = sMax(jntIdx); 69 | if taskScale > sStar 70 | sStar = taskScale; 71 | Wstar = W; 72 | dqNullStar = dqNull; 73 | end 74 | 75 | W(jntIdx, jntIdx) = 0; 76 | dqNull(jntIdx) = min(max(dqLow(jntIdx), dq(jntIdx)), dqUpp(jntIdx)); 77 | 78 | if rank(J*W) < nTask 79 | s = sStar; 80 | W = Wstar; 81 | dqNull = dqNullStar; 82 | dq = dqNull + pinv(J*W)*(s * dxGoal - J * dqNull); 83 | limitExceeded = false; 84 | end 85 | 86 | % this check is added for optimal version of algorithm 87 | % do this only if at least two joints are saturated 88 | if(nJnt > 2) 89 | if(sum(diag(W)) < nJnt-2) 90 | mu = -P'*dq; 91 | for i=1:nJnt 92 | if W(i,i) == 0 93 | if((abs(dq(i)-dqLow(i)) < 1e-6 && mu(i) > 0) || ... 94 | (abs(dq(i)-dqUpp(i)) < 1e-6 && mu(i) < 0)) 95 | % remove i-th joint from saturated joint set 96 | W(i,i) = 1; 97 | dqNull(i) = 0; 98 | limitExceeded = true; 99 | end 100 | end 101 | end 102 | end 103 | end 104 | end 105 | end 106 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_rr.m: -------------------------------------------------------------------------------- 1 | function [dq, s, exitCode] = snsIk_vel_rr(dqLow, dqUpp, dxGoal, J) 2 | % [dq, s, exitCode] = snsIk_vel_rr(dqLow, dqUpp, dxGoal, J) 3 | % 4 | % This function implements the basic version of the SNS-IK velocity 5 | % solver. 6 | % 7 | % INPUTS: 8 | % dqLow (nJnt x 1): lower limit for joint velocities 9 | % dqUpp (nJnt x 1): upper limit for joint velocities 10 | % dxGoal (ndx x 1): task-space velocity 11 | % J (ndx x nJnt): jacoabian mapping joint space to task space 12 | % 13 | % OUTPUTS: 14 | % dq (nJnt x 1): joint velocity solution with maximum task scale factor 15 | % s = task scale factor [0, 1] 16 | % exitCode (1 == success) 17 | % 18 | % 19 | % NOTES: 20 | % s * dxGoal = J * dq; 21 | % 22 | % This function is a modification of the basic SNS-IK algorithm that is 23 | % presented in the original SNS-IK papers. It was developed by Andy Park at 24 | % Rethink Robotics in June 2018. 25 | 26 | % Copyright 2018 Rethink Robotics 27 | % 28 | % Licensed under the Apache License, Version 2.0 (the "License"); 29 | % you may not use this file except in compliance with the License. 30 | % You may obtain a copy of the License at 31 | % http://www.apache.org/licenses/LICENSE-2.0 32 | % 33 | % Unless required by applicable law or agreed to in writing, software 34 | % distributed under the License is distributed on an "AS IS" BASIS, 35 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 36 | % See the License for the specific language governing permissions and 37 | % limitations under the License. 38 | 39 | % TODO: input validation 40 | 41 | [nTask, nJnt] = size(J); 42 | W = eye(nJnt); 43 | dqNull = zeros(nJnt, 1); 44 | s = 1.0; 45 | sStar = 0.0; 46 | exitCode = 1; 47 | tol = 1e-6; 48 | limitExceeded = true; 49 | while limitExceeded == true 50 | limitExceeded = false; 51 | dq = dqNull + pinv(J*W) * (dxGoal - J*dqNull); 52 | if any(dq < (dqLow - tol)) || any(dq > (dqUpp + tol)) 53 | limitExceeded = true; 54 | end 55 | a = pinv(J*W) * dxGoal; 56 | b = dq - a; 57 | 58 | marginL = dqLow - b; 59 | marginU = dqUpp - b; 60 | sMax = zeros(nJnt, 1); 61 | for i=1:nJnt 62 | if W(i,i) == 0 63 | sMax(i) = inf; 64 | else 65 | sMax(i) = FindScaleFactor(marginL(i), marginU(i), a(i)); 66 | end 67 | end 68 | 69 | [~, jntIdx] = min(sMax); 70 | taskScale = sMax(jntIdx); 71 | if taskScale > sStar 72 | sStar = taskScale; 73 | Wstar = W; 74 | dqNullStar = dqNull; 75 | end 76 | 77 | W(jntIdx, jntIdx) = 0; 78 | dqNull(jntIdx) = min(max(dqLow(jntIdx), dq(jntIdx)), dqUpp(jntIdx)); 79 | 80 | if rank(J*W) < nTask 81 | s = sStar; 82 | W = Wstar; 83 | dqNull = dqNullStar; 84 | dq = dqNull + pinv(J*W)*(s * dxGoal - J * dqNull); 85 | limitExceeded = false; 86 | end 87 | end 88 | 89 | end 90 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_rr_cs.m: -------------------------------------------------------------------------------- 1 | function [dq, s, sCS, exitCode] = snsIk_vel_rr_cs(dqLow, dqUpp, dxGoal, dqCS, J) 2 | % [dq, s, sCS, exitCode] = snsIk_vel_rr(dqLow, dqUpp, dxGoal, dqCS, J) 3 | % 4 | % This function implements a simplified multi-task version of the SNS-IK 5 | % acceleration solver that supports a secondary objective term. 6 | % In this function, the secondary task is assumed to be a desired 7 | % configuration-space velocity. 8 | % 9 | % INPUTS: 10 | % dqLow (nJnt x 1): lower limit for joint velocities 11 | % dqUpp (nJnt x 1): upper limit for joint velocities 12 | % dxGoal (ndx x 1): task-space velocity (primary goal) 13 | % dqCS (nJnt x 1): configuration space velocity (secondary goal) 14 | % J (ndx x nJnt): jacoabian mapping joint space to task space 15 | % 16 | % OUTPUTS: 17 | % dq (nJnt x 1): joint velocity solution with maximum task scale factor 18 | % s = task scale factor [0, 1] 19 | % sCS = nullspace configuration task scale factor [0, 1] 20 | % exitCode (1 == success) 21 | % 22 | % 23 | % NOTES: 24 | % s * dxGoal = J * dq; 25 | % sCS * dqCS = (I - pinv(J)*J) * dq; 26 | % 27 | % This function is a modification of the basic SNS-IK algorithm that is 28 | % presented in the original SNS-IK papers. It was developed by Andy Park at 29 | % Rethink Robotics in June 2018. 30 | 31 | % Copyright 2018 Rethink Robotics 32 | % 33 | % Licensed under the Apache License, Version 2.0 (the "License"); 34 | % you may not use this file except in compliance with the License. 35 | % You may obtain a copy of the License at 36 | % http://www.apache.org/licenses/LICENSE-2.0 37 | % 38 | % Unless required by applicable law or agreed to in writing, software 39 | % distributed under the License is distributed on an "AS IS" BASIS, 40 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 41 | % See the License for the specific language governing permissions and 42 | % limitations under the License. 43 | 44 | % TODO: input validation 45 | 46 | %% compute the solution for the primary task 47 | nJnt = size(J,2); 48 | tol = 1e-6; 49 | 50 | [dq, s, exitCode] = snsIk_vel_rr(dqLow, dqUpp, dxGoal, J); 51 | 52 | %% compute the solution for the secondary task 53 | %-- initialization 54 | dq1 = dq; 55 | I = eye(nJnt); 56 | Wcs = I; 57 | 58 | %-- start of the algorithm 59 | 60 | % set Wcs in order to use only non-saturated joints from the primary task 61 | for i=1:nJnt 62 | if (abs(dq1(i)-dqLow(i)) < tol || abs(dq1(i)-dqUpp(i)) < tol) 63 | Wcs(i,i) = 0; 64 | end 65 | end 66 | 67 | % compute nullspace projection matrices 68 | % P1: nullspace projection matrix of the primary task 69 | % Pcs ensures that the projected joint velocity does not 70 | % interfere with joint saturation and the primary task 71 | % tolerance is used for numerical stability 72 | P1 = (I - pinv(J)*J); 73 | Pcs = (I - pinv((I - Wcs)*P1, tol))*P1; 74 | 75 | % validate Pcs 76 | if (norm(J*Pcs) > tol && norm((I-Wcs)*Pcs) > tol) 77 | warning('Invalid Nullspace!'); 78 | J2 = [J; (I - Wcs)]; 79 | PcsTmp = I - pinv(J2)*J2; 80 | Pcs = PcsTmp; 81 | end 82 | 83 | aCS = Pcs * dqCS; 84 | bCS = dq1; 85 | 86 | % compute margins 87 | marginL = dqLow - bCS; 88 | marginU = dqUpp - bCS; 89 | 90 | % obtain scale factor candidates 91 | sCSMax = zeros(nJnt, 1); 92 | for i=1:nJnt 93 | if (Wcs(i,i) == 0) 94 | sCSMax(i) = inf; 95 | else 96 | sCSMax(i) = FindScaleFactor(marginL(i), marginU(i), aCS(i)); 97 | end 98 | end 99 | 100 | % find the most critical joint and scale factor 101 | [~, jntIdx] = min(sCSMax); 102 | sCS = sCSMax(jntIdx); 103 | 104 | if (isinf(sCS)) 105 | % if all joints are saturated, secondary task becomes infeasible! 106 | sCS = 0; 107 | end 108 | 109 | % compute the solution with the scaleFactor 110 | dq2 = dq1 + sCS*Pcs*dqCS; 111 | 112 | %-- end of algorithm 113 | 114 | dq = dq2; 115 | 116 | end 117 | -------------------------------------------------------------------------------- /sns_ik_lib/matlab/snsIk_vel_rr_mt.m: -------------------------------------------------------------------------------- 1 | function [dq, sData, exitCode] = snsIk_vel_rr_mt(dqLow, dqUpp, dxGoalData, JData) 2 | % [dq, sData, exitCode] = snsIk_vel_rr_mt(dqLow, dqUpp, dxGoalData, JData) 3 | % 4 | % This function implements a generic multi-task version of the SNS-IK 5 | % velocity solver that tries to meet multiple objective terms 6 | % in a prioritized manner. 7 | % 8 | % INPUTS: 9 | % dqLow (nJnt x 1): lower limit for joint velocities 10 | % dqUpp (nJnt x 1): upper limit for joint velocities 11 | % dxGoalData (cell array): task-space velocities of all the tasks in the order of priorities 12 | % JData (cell array): Jacobians of all the task in the order of priorities 13 | % 14 | % OUTPUTS: 15 | % dq (nJnt x 1): joint velocity solution with maximum task scale factor 16 | % sData (nTask x 1): task scale factors [0, 1] for all the tasks 17 | % exitCode (1 == success) 18 | % 19 | % NOTES: 20 | % 21 | % This function is a modification of the multi-task SNS-IK algorithm that is 22 | % presented in the original SNS-IK papers. It was developed by Andy Park at 23 | % Rethink Robotics in June 2018. 24 | 25 | % Copyright 2018 Rethink Robotics 26 | % 27 | % Licensed under the Apache License, Version 2.0 (the "License"); 28 | % you may not use this file except in compliance with the License. 29 | % You may obtain a copy of the License at 30 | % http://www.apache.org/licenses/LICENSE-2.0 31 | % 32 | % Unless required by applicable law or agreed to in writing, software 33 | % distributed under the License is distributed on an "AS IS" BASIS, 34 | % WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 35 | % See the License for the specific language governing permissions and 36 | % limitations under the License. 37 | 38 | % TODO: input validation 39 | 40 | % get the number of tasks 41 | nTask = size(JData,1); 42 | WData = cell(nTask,1); 43 | sData = zeros(nTask,1); 44 | dqData = cell(nTask,1); 45 | dqNullData = cell(nTask,1); 46 | 47 | % initialization 48 | exitCode = 1; 49 | tol = 1e-6; 50 | 51 | nJnt = numel(dqLow); 52 | I = eye(nJnt); 53 | Pi = I; 54 | dqi = zeros(nJnt,1); 55 | 56 | for iTask = 1:nTask 57 | 58 | % get i-th task jacobian 59 | Ji = JData{iTask}; 60 | ndxGoal = size(Ji,1); 61 | 62 | % get i-th task velocity 63 | dxGoali = dxGoalData{iTask}; 64 | 65 | % update variables for previous projection matrix and solution 66 | PiPrev = Pi; 67 | dqiPrev = dqi; 68 | 69 | % initialize variables for i-th task 70 | Wi = eye(nJnt); 71 | dqNulli = zeros(nJnt, 1); 72 | PiBar = PiPrev; 73 | si = 1.0; 74 | siStar = 0.0; 75 | 76 | limitExceeded = true; 77 | drop_correction_term = false; 78 | cntLoop = 1; 79 | while limitExceeded == true 80 | limitExceeded = false; 81 | 82 | % compute a solution without task scale factor 83 | PiHat = (I - pinv(Ji*PiBar, tol)*Ji)*pinv((I - Wi)*PiPrev, tol); 84 | 85 | if (drop_correction_term) 86 | dqi = dqiPrev + pinv(Ji*PiBar, tol) * dxGoali + ... 87 | PiHat*(dqNulli - dqiPrev); 88 | else 89 | dqi = dqiPrev + pinv(Ji*PiBar, tol) * (dxGoali - Ji*dqiPrev) + ... 90 | PiHat*(dqNulli - dqiPrev); 91 | end 92 | 93 | % check whether the solution violates the limits 94 | if any(dqi < (dqLow - tol)) || any(dqi > (dqUpp + tol)) 95 | limitExceeded = true; 96 | end 97 | 98 | % compute scale factor 99 | a = pinv(Ji*PiBar, tol) * dxGoali; 100 | b = dqi - a; 101 | 102 | marginL = dqLow - b; 103 | marginU = dqUpp - b; 104 | sMax = zeros(nJnt, 1); 105 | for iJnt=1:nJnt 106 | if Wi(iJnt,iJnt) == 0 107 | sMax(iJnt) = inf; 108 | else 109 | sMax(iJnt) = FindScaleFactor(marginL(iJnt), marginU(iJnt), a(iJnt)); 110 | end 111 | end 112 | 113 | [~, jntIdx] = min(sMax); 114 | taskScale = sMax(jntIdx); 115 | 116 | if (isinf(taskScale)) 117 | % this means that all joints are saturated, so 118 | % lower priority tasks become infeasible 119 | taskScale = 0; 120 | end 121 | 122 | % do the following only if the task is feasible and the scale 123 | % factor caculated is correct 124 | if (iTask == 1 || taskScale > 0) 125 | 126 | % try to store maximum scale factor with associated variables 127 | if taskScale > siStar 128 | siStar = taskScale; 129 | Wistar = Wi; 130 | dqNulliStar = dqNulli; 131 | PiBarStar = PiBar; 132 | PiHatStar = PiHat; 133 | end 134 | 135 | % saturate the most critical joint 136 | Wi(jntIdx, jntIdx) = 0; 137 | dqNulli(jntIdx) = min(max(dqLow(jntIdx), dqi(jntIdx)), dqUpp(jntIdx)); 138 | 139 | % update projection matrices 140 | PiBar = (I - pinv((I - Wi)*PiPrev, tol))*PiPrev; 141 | PiHat = (I - pinv(Ji*PiBar, tol)*Ji)*pinv((I - Wi)*PiPrev, tol); 142 | 143 | % if rank is below task dimension, terminate the loop and output 144 | % the current best solution 145 | if (rank(Ji*PiBar) < ndxGoal) 146 | si = siStar; 147 | Wi = Wistar; 148 | dqNulli = dqNulliStar; 149 | PiBar = PiBarStar; 150 | PiHat = PiHatStar; 151 | 152 | if (drop_correction_term) 153 | dqi = dqiPrev + pinv(Ji*PiBar, tol) * si*dxGoali + ... 154 | PiHat*(dqNulli - dqiPrev); 155 | 156 | limitExceeded = false; 157 | else 158 | dqi = dqiPrev + pinv(Ji*PiBar, tol) * (si*dxGoali - Ji*dqiPrev) + ... 159 | PiHat*(dqNulli - dqiPrev); 160 | 161 | % if the solution violates the limits, drop the 162 | % correction term and run it again 163 | if (any(dqi > dqUpp + tol) || any(dqi < dqLow - tol)) 164 | drop_correction_term = true; 165 | cntLoop = 0; 166 | Wi = eye(nJnt); 167 | dqNulli = zeros(nJnt, 1); 168 | PiBar = PiPrev; 169 | si = 1.0; 170 | siStar = 0.0; 171 | else 172 | limitExceeded = false; 173 | end 174 | end 175 | 176 | end 177 | 178 | else % if the current task is infeasible 179 | si = 0; 180 | Wi = zeros(nJnt, nJnt); 181 | dqi = dqiPrev; 182 | limitExceeded = false; 183 | end 184 | 185 | cntLoop = cntLoop + 1; 186 | end 187 | 188 | if (si > 0) 189 | % update nullspace projection 190 | Pi = PiPrev - pinv(Ji*PiPrev, tol)*(Ji*PiPrev); 191 | end 192 | 193 | % store data 194 | WData{iTask} = Wi; 195 | sData(iTask) = si; 196 | dqData{iTask} = dqi; 197 | dqNullData{iTask} = dqNulli; 198 | end 199 | 200 | %-- end of algorithm 201 | dq = dqi; 202 | 203 | end 204 | -------------------------------------------------------------------------------- /sns_ik_lib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sns_ik_lib 4 | 0.2.3 5 | 6 | Saturation in the Null Space (SNS) Inverse Kinematic Library. 7 | SNS is a real-time Cartesian IK solver for uses the redundancy 8 | in the supplied kinematic chain for collision avoidance, 9 | joint motion optimization, or additional task objectives or 10 | soft constraints. 11 | 12 | 13 | Fabrizio Flacco 14 | Ian McMahon 15 | Forrest Rogers-Marcovitz 16 | 17 | Rethink Robotics Inc. 18 | 19 | Apache 2.0 20 | http://ros.org/wiki/sns_ikl 21 | 22 | https://github.com/RethinkRobotics-opensource/sns_ikl 23 | 24 | 25 | https://github.com/RethinkRobotics-opensource/sns_ikl/issues 26 | 27 | 28 | catkin 29 | 30 | std_msgs 31 | roscpp 32 | eigen 33 | kdl_parser 34 | orocos_kdl 35 | 36 | std_msgs 37 | roscpp 38 | orocos_kdl 39 | 40 | 41 | -------------------------------------------------------------------------------- /sns_ik_lib/src/osns_sm_velocity_ik.cpp: -------------------------------------------------------------------------------- 1 | /*! \file osns_sm_velocity_ik.hpp 2 | * \brief Optimal SNS, with scale margin, velocity IK solver 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | */ 6 | /* 7 | * Copyright 2016 Rethink Robotics 8 | * 9 | * Copyright 2012-2016 Fabrizio Flacco 10 | * 11 | * Licensed under the Apache License, Version 2.0 (the "License"); 12 | * you may not use this file except in compliance with the License. 13 | * You may obtain a copy of the License at 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, software 17 | * distributed under the License is distributed on an "AS IS" BASIS, 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | * See the License for the specific language governing permissions and 20 | * limitations under the License. 21 | */ 22 | 23 | #include 24 | 25 | #include "sns_ik_math_utils.hpp" 26 | 27 | namespace sns_ik { 28 | 29 | OSNS_sm_VelocityIK::OSNS_sm_VelocityIK(int dof, double loop_period) : 30 | OSNSVelocityIK(dof, loop_period), 31 | m_scaleMargin(0.9) 32 | { 33 | } 34 | 35 | 36 | double OSNS_sm_VelocityIK::getJointVelocity(Eigen::VectorXd *jointVelocity, 37 | const std::vector &sot, 38 | const Eigen::VectorXd &jointConfiguration) 39 | { 40 | // This will only reset member variables if different from previous values 41 | setNumberOfTasks(sot.size(), sot[0].jacobian.cols()); 42 | 43 | // TODO: check that setJointsCapabilities has been already called 44 | 45 | //P_0=I 46 | //dq_0=0 47 | Eigen::MatrixXd P = Eigen::MatrixXd::Identity(n_dof, n_dof); 48 | *jointVelocity = Eigen::VectorXd::Zero(n_dof, 1); 49 | Eigen::VectorXd higherPriorityJointVelocity; 50 | Eigen::MatrixXd higherPriorityNull; 51 | 52 | shapeJointVelocityBound(jointConfiguration, m_scaleMargin); 53 | 54 | Eigen::MatrixXd PS = Eigen::MatrixXd::Identity(n_dof, n_dof); 55 | 56 | for (int i_task = 0; i_task < n_tasks; i_task++) { //consider all tasks 57 | higherPriorityJointVelocity = *jointVelocity; 58 | higherPriorityNull = P; 59 | 60 | scaleFactors[i_task] = SNSsingle(i_task, higherPriorityJointVelocity, higherPriorityNull, 61 | sot[i_task].jacobian, sot[i_task].desired, jointVelocity, &PS); 62 | 63 | if (scaleFactors[i_task] < 0) { 64 | //second chance 65 | W[i_task] = I; 66 | PS = higherPriorityNull; 67 | scaleFactors[i_task] = SNSsingle(i_task, higherPriorityJointVelocity, higherPriorityNull, 68 | sot[i_task].jacobian, sot[i_task].desired, jointVelocity, &PS); 69 | 70 | } 71 | 72 | if (scaleFactors[i_task] > 0.0) { 73 | if (scaleFactors[i_task] * m_scaleMargin < (1.0)) { 74 | double taskScale = scaleFactors[i_task] * m_scaleMargin; 75 | Eigen::VectorXd scaledTask = sot[i_task].desired * taskScale; 76 | scaleFactors[i_task] = SNSsingle(i_task, higherPriorityJointVelocity, higherPriorityNull, 77 | sot[i_task].jacobian, scaledTask, jointVelocity, &P); 78 | scaleFactors[i_task] = taskScale; 79 | 80 | } else { 81 | scaleFactors[i_task] = 1.0; 82 | P = PS; 83 | } 84 | } 85 | } 86 | 87 | return 1.0; 88 | } 89 | 90 | } // namespace sns_ik 91 | -------------------------------------------------------------------------------- /sns_ik_lib/src/sns_position_ik.cpp: -------------------------------------------------------------------------------- 1 | /*! \file sns_position_ik.cpp 2 | * \brief Basic SNS Position IK solver 3 | * \author Forrest Rogers-Marcovitz 4 | */ 5 | /* 6 | * Copyright 2016 Rethink Robotics 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, 15 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 16 | * See the License for the specific language governing permissions and 17 | * limitations under the License. 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "sns_ik_math_utils.hpp" 25 | 26 | namespace sns_ik { 27 | 28 | SNSPositionIK::SNSPositionIK(KDL::Chain chain, std::shared_ptr velocity_ik, double eps) : 29 | m_chain(chain), 30 | m_ikVelSolver(velocity_ik), 31 | m_positionFK(chain), 32 | m_jacobianSolver(chain), 33 | m_linearMaxStepSize(0.2), 34 | m_angularMaxStepSize(0.2), 35 | m_maxIterations(150), 36 | m_eps(eps), 37 | m_dt(0.2), 38 | m_useBarrierFunction(true), 39 | m_barrierInitAlpha(0.1), 40 | m_barrierDecay(0.8) 41 | { 42 | } 43 | 44 | SNSPositionIK::~SNSPositionIK() 45 | { 46 | } 47 | 48 | bool SNSPositionIK::calcPoseError(const KDL::JntArray& q, 49 | const KDL::Frame& goal, 50 | KDL::Frame* pose, 51 | double* errL, 52 | double* errR, 53 | KDL::Vector* trans, 54 | KDL::Vector* rotAxis) 55 | { 56 | if (m_positionFK.JntToCart(q, *pose) < 0) 57 | { 58 | ROS_ERROR("JntToCart failed"); 59 | return false; 60 | } 61 | 62 | // Calculate the offset transform 63 | *trans = goal.p - pose->p; 64 | *errL = trans->Norm(); 65 | KDL::Rotation rot = goal.M * pose->M.Inverse(); 66 | *errR = rot.GetRotAngle(*rotAxis); // returns [0 ... pi] 67 | return true; 68 | } 69 | 70 | int SNSPositionIK::CartToJnt(const KDL::JntArray& joint_seed, 71 | const KDL::Frame& goal_pose, 72 | const KDL::JntArray& joint_ns_bias, 73 | const Eigen::MatrixXd& ns_jacobian, 74 | const std::vector& ns_indicies, 75 | const double ns_gain, 76 | KDL::JntArray* return_joints, 77 | const KDL::Twist& bounds) 78 | { 79 | Eigen::VectorXd jl_low = m_ikVelSolver->getJointLimitLow(); 80 | Eigen::VectorXd jl_high = m_ikVelSolver->getJointLimitHigh(); 81 | Eigen::VectorXd maxJointVel = m_ikVelSolver->getJointVelocityMax(); 82 | 83 | // initialize variables 84 | bool solutionFound = false; 85 | KDL::JntArray q_i = joint_seed; 86 | KDL::Frame pose_i; 87 | int n_dof = joint_seed.rows(); 88 | std::vector sot(1); 89 | sot[0].desired = Eigen::VectorXd::Zero(6); 90 | 91 | // If there's a nullspace bias, create a secondary task 92 | if (joint_ns_bias.rows()) { 93 | Task nsTask; 94 | nsTask.jacobian = ns_jacobian; 95 | nsTask.desired = Eigen::VectorXd::Zero(joint_ns_bias.rows()); 96 | // the desired task to apply the NS bias will change with each iteration 97 | sot.push_back(nsTask); 98 | } 99 | 100 | double theta; 101 | double lineErr, rotErr; 102 | Eigen::VectorXd qDot(n_dof); 103 | KDL::Jacobian jacobian; 104 | jacobian.resize(q_i.rows()); 105 | KDL::Vector rotAxis, trans; 106 | KDL::Rotation rot; 107 | KDL::Twist delta_twist; 108 | 109 | double barrierAlpha = m_barrierInitAlpha; 110 | 111 | int ii; 112 | for (ii = 0; ii < m_maxIterations; ++ii) { 113 | 114 | if (!calcPoseError(q_i, goal_pose, &pose_i, &lineErr, &rotErr, &trans, &rotAxis)) { 115 | ROS_ERROR("Failed to calculate pose error!"); 116 | return -1; 117 | } 118 | 119 | // Check stopping tolerances 120 | delta_twist = diffRelative(goal_pose, pose_i); 121 | 122 | if (std::abs(delta_twist.vel.x()) <= std::abs(bounds.vel.x())) 123 | delta_twist.vel.x(0); 124 | if (std::abs(delta_twist.vel.y()) <= std::abs(bounds.vel.y())) 125 | delta_twist.vel.y(0); 126 | if (std::abs(delta_twist.vel.z()) <= std::abs(bounds.vel.z())) 127 | delta_twist.vel.z(0); 128 | if (std::abs(delta_twist.rot.x()) <= std::abs(bounds.rot.x())) 129 | delta_twist.rot.x(0); 130 | if (std::abs(delta_twist.rot.y()) <= std::abs(bounds.rot.y())) 131 | delta_twist.rot.y(0); 132 | if (std::abs(delta_twist.rot.z()) <= std::abs(bounds.rot.z())) 133 | delta_twist.rot.z(0); 134 | 135 | if(KDL::Equal(delta_twist, KDL::Twist::Zero(), m_eps)) { 136 | solutionFound = true; 137 | break; 138 | } 139 | 140 | // Enforce max linear and rotational step sizes 141 | if (lineErr > m_linearMaxStepSize) { 142 | trans = (m_linearMaxStepSize / lineErr) * trans; 143 | } 144 | 145 | theta = rotErr; 146 | if (theta > m_angularMaxStepSize) { 147 | theta = m_angularMaxStepSize; 148 | } 149 | 150 | // Calculate the desired Cartesian twist 151 | sot[0].desired(0) = trans.data[0] / m_dt; 152 | sot[0].desired(1) = trans.data[1] / m_dt; 153 | sot[0].desired(2) = trans.data[2] / m_dt; 154 | sot[0].desired(3) = theta * rotAxis.data[0] / m_dt; 155 | sot[0].desired(4) = theta * rotAxis.data[1] / m_dt; 156 | sot[0].desired(5) = theta * rotAxis.data[2] / m_dt; 157 | 158 | if (m_jacobianSolver.JntToJac(q_i, jacobian) < 0) 159 | { 160 | ROS_ERROR("JntToJac failed"); 161 | return -1; 162 | } 163 | sot[0].jacobian = jacobian.data; 164 | 165 | if (joint_ns_bias.rows()) { 166 | for (size_t jj = 0; jj < joint_ns_bias.rows(); ++jj) { 167 | // This calculates a "nullspace velocity". 168 | // There is an arbitrary scale factor which will be set by the max scale factor. 169 | int indx = ns_indicies[jj]; 170 | double vel = ns_gain * (joint_ns_bias(jj) - q_i(indx)); // TODO: step size needs to be optimized 171 | // TODO: may want to limit the NS velocity to 50% of max joint velocity 172 | //vel = std::max(-0.5*maxJointVel(indx), std::min(0.5*maxJointVel(indx), vel)); 173 | sot[1].desired(jj) = vel; 174 | } 175 | 176 | } 177 | 178 | m_ikVelSolver->getJointVelocity(&qDot, sot, q_i.data); 179 | 180 | if (qDot.norm() < 1e-6) { // TODO: config param 181 | ROS_ERROR("qDot.norm() too small!"); 182 | return -2; 183 | } 184 | 185 | // Update the joint positions 186 | q_i.data += m_dt * qDot; 187 | 188 | // Apply a decaying barrier function 189 | // u = upper limit; l = lower limit 190 | // B(x) = -log(u - x) - log(-l + x) 191 | // -alpha * dB(x)/dx === alpha * (1/(x-l) + 1/(x-u)) 192 | if (m_useBarrierFunction && (lineErr > 0.5 || rotErr > 0.5) ) { 193 | for (int j = 0; j < jl_low.rows(); ++j) { 194 | // First force the joint within limits. 195 | // It can not be exactly at the limit since it will cause a division by zero NaN in the barrier function. 196 | q_i.data[j] = std::max(std::min(q_i.data[j], jl_high[j] - 1e-7), jl_low[j] + 1e-7); 197 | q_i.data[j] += barrierAlpha * (1/(q_i.data[j] - jl_low[j]) + 1/(q_i.data[j] - jl_high[j])); 198 | } 199 | barrierAlpha *= m_barrierDecay; 200 | } else { 201 | for (int j = 0; j < jl_low.rows(); ++j) { 202 | q_i.data[j] = std::max(std::min(q_i.data[j], jl_high[j]), jl_low[j]); 203 | } 204 | } 205 | } 206 | 207 | if (solutionFound) { 208 | *return_joints = q_i; 209 | ROS_DEBUG("Solution Found in %d iterations!", ii); 210 | return 1; // TODO: return success/fail code 211 | } else { 212 | return -1; 213 | } 214 | } 215 | 216 | } // namespace sns_ik 217 | -------------------------------------------------------------------------------- /sns_ik_lib/src/sns_vel_ik_base_interface.cpp: -------------------------------------------------------------------------------- 1 | /*! \file sns_velocity_base_ik.cpp 2 | * \brief Basic SNS velocity IK solver (Rethink version) 3 | * \author Fabrizio Flacco 4 | * \author Forrest Rogers-Marcovitz 5 | * \author Andy Park 6 | */ 7 | /* 8 | * Copyright 2018 Rethink Robotics 9 | * 10 | * Copyright 2012-2016 Fabrizio Flacco 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | */ 23 | 24 | #include 25 | #include 26 | #include "sns_ik_math_utils.hpp" 27 | 28 | namespace sns_ik { 29 | 30 | SNSVelIKBaseInterface::SNSVelIKBaseInterface(int dof, double loop_period) : 31 | SNSVelocityIK(dof, loop_period) 32 | { 33 | baseIkSolver = SnsVelIkBase::create(n_dof); 34 | } 35 | 36 | double SNSVelIKBaseInterface::getJointVelocity(Eigen::VectorXd *jointVelocity, 37 | const std::vector &sot, 38 | const Eigen::VectorXd &jointConfiguration) 39 | { 40 | // This will only reset member variables if different from previous values 41 | setNumberOfTasks(sot.size(), sot[0].jacobian.cols()); 42 | 43 | // calculate box constraints 44 | shapeJointVelocityBound(jointConfiguration); 45 | 46 | // solve using SNS base IK solver (andy) 47 | dqLow = dotQmin; 48 | dqUpp = dotQmax; 49 | J = sot[0].jacobian; 50 | dx = sot[0].desired; 51 | dqSol.resize(n_dof); 52 | taskScale = 1.0; 53 | 54 | if (n_tasks > 1) { 55 | // if the tasks include a nullspace bias task 56 | taskScaleCS = 1.0; 57 | dqCS = sot[1].desired; 58 | } 59 | 60 | // set box constraints 61 | baseIkSolver->setBounds(dqLow, dqUpp); 62 | 63 | if (n_tasks == 1) { 64 | exitCode = baseIkSolver->solve(J, dx, &dqSol, &taskScale); 65 | scaleFactors[0] = taskScale; 66 | } 67 | else { 68 | exitCode = baseIkSolver->solve(J, dx, dqCS, &dqSol, &taskScale, &taskScaleCS); 69 | scaleFactors[0] = taskScale; 70 | scaleFactors[1] = taskScaleCS; 71 | } 72 | 73 | // store solution and scale factor 74 | *jointVelocity = dqSol; 75 | 76 | // return -1.0 when IK was not successful 77 | if (exitCode != SnsIkBase::ExitCode::Success) 78 | return -1.0; 79 | 80 | return 1.0; 81 | } 82 | 83 | 84 | 85 | } // namespace sns_ik -------------------------------------------------------------------------------- /sns_ik_lib/test/rng_utilities.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Rethink Robotics 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | #include "rng_utilities.hpp" 17 | 18 | #include 19 | #include 20 | 21 | namespace sns_ik { 22 | namespace rng_util { 23 | 24 | /*************************************************************************************************/ 25 | 26 | void setRngSeed(int seedDouble, int seedInt) { 27 | getRngDouble(seedDouble); 28 | getRngInt(seedInt); 29 | } 30 | 31 | /*************************************************************************************************/ 32 | 33 | double getRngDouble(int seed, double low, double upp) { 34 | static std::mt19937 gen; // mersenne-twister pseudo-random number generator 35 | if (upp <= low) { return low; } // catch edge case 36 | if (seed > 0) { gen.seed(seed); } // set the seed so that we get repeatable outputs for the test 37 | static std::uniform_real_distribution<> dis(0.0, 1.0); 38 | return low + (upp - low) * dis(gen); // sample the RNG and then map to the range [0, 1] 39 | } 40 | 41 | /*************************************************************************************************/ 42 | 43 | int getRngInt(int seed, int low, int upp) { 44 | if (upp <= low) { return low; } // catch edge case 45 | if (seed != 0) { srand(seed); } // set the seed so that we get repeatable outputs 46 | return rand() % (upp-low) + low; // not a true uniform distribution, but close enough 47 | } 48 | 49 | /*************************************************************************************************/ 50 | 51 | Eigen::MatrixXd getRngMatrixXd(int seed, int nRows, int nCols, double low, double upp) 52 | { 53 | Eigen::MatrixXd data(nRows, nCols); 54 | if (nRows < 1 || nCols < 1) { return data; } 55 | for (int iRow = 0; iRow < nRows; iRow++) { 56 | for (int iCol = 0; iCol < nCols; iCol++) { 57 | if (iRow == 0 && iCol == 0) { // Set the seed in the RNG on first call 58 | data(iRow, iCol) = getRngDouble(seed, low, upp); 59 | } else { // Do not update the seed 60 | data(iRow, iCol) = getRngDouble(0, low, upp); // 0 == do not update seed 61 | } 62 | } 63 | } 64 | return data; 65 | } 66 | 67 | /*************************************************************************************************/ 68 | 69 | Eigen::MatrixXd getRngMatrixXdRanked(int seed, int nRows, int nCols, int nRank) 70 | { 71 | nRows = std::max(nRows, 1); 72 | nCols = std::max(nCols, 1); 73 | nRank = std::max(nRank, 1); 74 | nRank = std::min({nRows, nCols, nRank}); 75 | Eigen::MatrixXd A = getRngMatrixXd(seed + 67394, nRows, nRank, -1.0, 1.0); 76 | Eigen::MatrixXd B = getRngMatrixXd(seed + 78895, nRank, nCols, -1.0, 1.0); 77 | Eigen::MatrixXd X = A * B; 78 | return X; 79 | } 80 | 81 | /*************************************************************************************************/ 82 | 83 | Eigen::ArrayXd getRngArrBndXd(int seed, const Eigen::ArrayXd& low, const Eigen::ArrayXd& upp) 84 | { 85 | int n = std::min(low.size(), upp.size()); 86 | Eigen::ArrayXd data(n); 87 | for (int i = 0; i < n; i++) { 88 | data(i) = getRngDouble(seed, low(i), upp(i)); 89 | seed = 0; // automatically update RNG after first call 90 | } 91 | return data; 92 | } 93 | 94 | /*************************************************************************************************/ 95 | 96 | KDL::JntArray getRngBoundedJoints(int seed, const KDL::JntArray& qLow, const KDL::JntArray& qUpp) 97 | { 98 | if (qUpp.rows() != qLow.rows()) { ROS_ERROR("Invalid input!"); return qLow; } 99 | int nJnt = qLow.rows(); 100 | KDL::JntArray q(nJnt); 101 | for (int iJnt = 0; iJnt < nJnt; iJnt++) { 102 | q(iJnt) = getRngDouble(seed, qLow(iJnt), qUpp(iJnt)); 103 | seed = 0; // let the RNG update the sequence automatically after the first call 104 | } 105 | return q; 106 | } 107 | 108 | /*************************************************************************************************/ 109 | 110 | KDL::JntArray getNearbyJoints(int seed, const KDL::JntArray& qNom, double delta, 111 | const KDL::JntArray& qLow, const KDL::JntArray& qUpp) 112 | { 113 | if (qLow.rows() != qNom.rows()) { ROS_ERROR("Invalid input!"); return qNom; } 114 | if (qUpp.rows() != qNom.rows()) { ROS_ERROR("Invalid input!"); return qNom; } 115 | int nJnt = qLow.rows(); 116 | KDL::JntArray q(nJnt); 117 | for (int iJnt = 0; iJnt < nJnt; iJnt++) { 118 | double low = std::max(qNom(iJnt) - delta, qLow(iJnt)); 119 | double upp = std::min(qNom(iJnt) + delta, qUpp(iJnt)); 120 | q(iJnt) = getRngDouble(seed, low, upp); 121 | seed = 0; // let the RNG update the sequence automatically after the first call 122 | } 123 | return q; 124 | } 125 | 126 | /*************************************************************************************************/ 127 | 128 | } // namespace rng_util 129 | } // namespace sns_ik 130 | -------------------------------------------------------------------------------- /sns_ik_lib/test/sawyer_model.cpp: -------------------------------------------------------------------------------- 1 | /** @file sawyer_model.cpp 2 | * 3 | * @brief The file provides simple functions to return robot models for testing. 4 | * 5 | * @author Matthew Kelly 6 | * 7 | * This file provides a set of functions that return simple kinematic chains that are used for the 8 | * unit tests. This allows unit tests to run quickly without depending on external URDF files. 9 | * 10 | * Copyright 2018 Rethink Robotics 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | */ 23 | 24 | #include "sawyer_model.hpp" 25 | 26 | #include 27 | 28 | namespace sns_ik { 29 | namespace sawyer_model { 30 | 31 | /*************************************************************************************************/ 32 | 33 | KDL::Chain getSawyerKdlChain(std::vector* jointNames) 34 | { 35 | if (jointNames){ 36 | *jointNames = {"right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5", "right_j6"}; 37 | } 38 | 39 | using namespace KDL; 40 | Chain chain; 41 | 42 | chain.addSegment(Segment("right_arm_mount", Joint(Joint::None), KDL::Frame(Rotation::RPY(0.0,0.0,0.0), Vector(0.0, 0.0, 0.08)))); 43 | chain.addSegment(Segment("right_l0", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(-1.57079632679, 1.57079632679, 0), Vector(0.081, 0.05, 0.237)))); 44 | chain.addSegment(Segment("right_l1", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(1.57079632679, 0, 0), Vector(0.0, -0.14, 0.1425)))); 45 | chain.addSegment(Segment("right_l2", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(-1.57079632679, 0, 0), Vector(0.0, -0.042, 0.26)))); 46 | chain.addSegment(Segment("right_l3", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(1.57079632679, 0, 0), Vector(0.0, -0.125, -0.1265)))); 47 | chain.addSegment(Segment("right_l4", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(-1.57079632679, 0, 0), Vector(0.0, 0.031, 0.275)))); 48 | chain.addSegment(Segment("right_l5", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(-1.57079632679, -0.17453, 3.1416), Vector(0.0, -0.11, 0.1053)))); 49 | chain.addSegment(Segment("right_l6", Joint(Joint::RotZ), KDL::Frame(Rotation::RPY(0, 0, 1.570796), Vector(0, 0, 0.0245)))); 50 | chain.addSegment(Segment("right_hand", Joint(Joint::None), KDL::Frame(Rotation::RPY(0.0,0.0,0.0), Vector(0.0, 0.0, 0.0)))); 51 | 52 | return chain; 53 | } 54 | 55 | /*************************************************************************************************/ 56 | 57 | void getSawyerJointLimits(KDL::JntArray* qLow, KDL::JntArray* qUpp, 58 | KDL::JntArray* vMax, KDL::JntArray* aMax) 59 | { 60 | if (!qLow || !qUpp || !vMax || !aMax) { ROS_ERROR("Bad input!"); return; } 61 | int nJnt = 7; // Sawer has seven joints 62 | *qLow = KDL::JntArray(nJnt); 63 | *qUpp = KDL::JntArray(nJnt); 64 | *vMax = KDL::JntArray(nJnt); 65 | *aMax = KDL::JntArray(nJnt); 66 | (*qLow)(0) = -3.0503; 67 | (*qUpp)(0) = 3.0503; 68 | (*vMax)(0) = 1.6; 69 | (*aMax)(0) = 8.0; 70 | (*qLow)(1) = -3.8095; 71 | (*qUpp)(1) = 2.2736; 72 | (*vMax)(1) = 1.3; 73 | (*aMax)(1) = 8.0; 74 | (*qLow)(2) = -3.0426; 75 | (*qUpp)(2) = 3.0426; 76 | (*vMax)(2) = 1.9; 77 | (*aMax)(2) = 8.0; 78 | (*qLow)(3) = -3.0439; 79 | (*qUpp)(3) = 3.0439; 80 | (*vMax)(3) = 1.9; 81 | (*aMax)(3) = 8.0; 82 | (*qLow)(4) = -2.9761; 83 | (*qUpp)(4) = 2.9761; 84 | (*vMax)(4) = 3.4; 85 | (*aMax)(4) = 10.0; 86 | (*qLow)(5) = -2.9761; 87 | (*qUpp)(5) = 2.9761; 88 | (*vMax)(5) = 3.4; 89 | (*aMax)(5) = 10.0; 90 | (*qLow)(6) = -3.14; 91 | (*qUpp)(6) = 3.14; 92 | (*vMax)(6) = 4.5; 93 | (*aMax)(6) = 10.0; 94 | } 95 | 96 | /*************************************************************************************************/ 97 | 98 | } // namespace sns_ik 99 | } // namespace sawyer_model 100 | -------------------------------------------------------------------------------- /sns_ik_lib/test/sawyer_model.hpp: -------------------------------------------------------------------------------- 1 | /** @file sawyer_model.hpp 2 | * 3 | * @brief The file provides simple functions to return robot models for testing. 4 | * 5 | * @author Matthew Kelly 6 | * 7 | * This file provides a set of functions that return simple kinematic chains that are used for the 8 | * unit tests. This allows unit tests to run quickly without depending on external URDF files. 9 | * 10 | * Copyright 2018 Rethink Robotics 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the "License"); 13 | * you may not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * http://www.apache.org/licenses/LICENSE-2.0 16 | * 17 | * Unless required by applicable law or agreed to in writing, software 18 | * distributed under the License is distributed on an "AS IS" BASIS, 19 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | * See the License for the specific language governing permissions and 21 | * limitations under the License. 22 | */ 23 | 24 | 25 | #ifndef SNS_IK_ROBOT_MODELS_H_ 26 | #define SNS_IK_ROBOT_MODELS_H_ 27 | 28 | #include 29 | #include 30 | 31 | namespace sns_ik { 32 | namespace sawyer_model { 33 | 34 | /* 35 | * This function returns a KDL chain object that represents the kinematics of a Sawyer Robot arm. 36 | * Full model description at: https://github.com/RethinkRobotics/sawyer_robot 37 | * @param[out] jointNames: names of all non-trivial joints 38 | * @return: kinematic chain between right_arm_mount and right_hand for Sawyer. 39 | */ 40 | KDL::Chain getSawyerKdlChain(std::vector* jointNames); 41 | 42 | /* 43 | * This function returns the joint limits for the sawyer robot arm 44 | * Full model description at: https://github.com/RethinkRobotics/sawyer_robot 45 | * @param[out] qLow: lower bound on joint angles 46 | * @param[out] qUpp: upper bound on joint angles 47 | * @param[out] vMax: maximum joint speed (symmetric) 48 | * @param[out] aMax: maximum joint acceleration (symmetric) 49 | */ 50 | void getSawyerJointLimits(KDL::JntArray* qLow, KDL::JntArray* qUpp, 51 | KDL::JntArray* vMax, KDL::JntArray* aMax); 52 | 53 | } // namespace sns_ik 54 | } // namespace sawyer_model 55 | 56 | #endif // SNS_IK_ROBOT_MODELS_H_ 57 | -------------------------------------------------------------------------------- /sns_ik_lib/test/test_utilities.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018 Rethink Robotics 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "test_utilities.hpp" 23 | 24 | namespace sns_ik { 25 | namespace test_util { 26 | 27 | /*************************************************************************************************/ 28 | 29 | void checkEqualVector(const Eigen::VectorXd& A, const Eigen::VectorXd& B, double tol) 30 | { 31 | ASSERT_EQ(A.size(), B.size()); 32 | int n = A.size(); 33 | for (int i = 0; i < n; i++) { 34 | ASSERT_NEAR(A(i), B(i), tol); 35 | } 36 | } 37 | 38 | /*************************************************************************************************/ 39 | 40 | void checkVectorLimits(const Eigen::VectorXd& low, const Eigen::VectorXd& val, const Eigen::VectorXd& upp, double tol) 41 | { 42 | ASSERT_EQ(low.size(), val.size()); 43 | ASSERT_EQ(upp.size(), val.size()); 44 | int n = val.size(); 45 | for (int i = 0; i < n; i++) { 46 | ASSERT_LE(val(i), upp(i) + tol); 47 | ASSERT_GE(val(i), low(i) - tol); 48 | } 49 | } 50 | 51 | /*************************************************************************************************/ 52 | 53 | } // namespace test_util 54 | } // namespace sns_ik -------------------------------------------------------------------------------- /sns_ik_lib/test/test_utilities.hpp: -------------------------------------------------------------------------------- 1 | /** @file test_utilities.hpp 2 | * 3 | * @brief a collection of functions for testing Eigen type vector data 4 | * 5 | * @author Matthew Kelly 6 | * @author Andy Park 7 | * 8 | * This file provides a set of functions that are used to generate repeatable pseudo-random 9 | * data for unit tests. All functions provide the caller with direct control over the seed that 10 | * is used by the random number generator. Passing a seed of zero is used to indicate that the 11 | * seed should not be reset, instead letting the random generator progress to the next value in 12 | * the sequence. This is useful for generating numbers in a loop, such as for a matrix. 13 | * 14 | * Copyright 2018 Rethink Robotics 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | */ 27 | 28 | #ifndef SNS_IK_LIB_TEST_UTILITIES_H 29 | #define SNS_IK_LIB_TEST_UTILITIES_H 30 | 31 | #include 32 | 33 | namespace sns_ik { 34 | namespace test_util { 35 | 36 | /** 37 | * Check if the elements in vector A and B are equal 38 | */ 39 | void checkEqualVector(const Eigen::VectorXd& A, const Eigen::VectorXd& B, double tol); 40 | 41 | /** 42 | * Check if the elements in a vector are within lower and upper limits 43 | */ 44 | void checkVectorLimits(const Eigen::VectorXd& low, const Eigen::VectorXd& val, const Eigen::VectorXd& upp, double tol); 45 | 46 | } // namespace test_util 47 | } // namespace sns_ik 48 | 49 | #endif //SNS_IK_LIB_TEST_UTILITIES_H -------------------------------------------------------------------------------- /sns_ik_lib/utilities/sns_linear_solver.cpp: -------------------------------------------------------------------------------- 1 | /** @file sns_linear_solver.cpp 2 | * 3 | * @brief Linear Solver, used by the SNS-IK velocity IK solvers. 4 | * @author: Matthew Kelly 5 | */ 6 | 7 | /** 8 | * Copyright 2018 Rethink Robotics 9 | * 10 | * Licensed under the Apache License, Version 2.0 (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License at 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | /* 23 | * 24 | * This class is used to solve linear systems. It is a wrapper for two different internal solvers: 25 | * --> psuedo-inverse solver: included for legacy support on Eigen 3.2.0 26 | * --> direct linear solver: prefered solver when available. Requries Eigen 3.3.4 27 | */ 28 | 29 | #include "sns_linear_solver.hpp" 30 | 31 | #if EIGEN_VERSION_AT_LEAST(3,3,4) // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // 32 | // Eigen version is newer than 3.3.4: CompleteOrthogonalDecomposition is defined 33 | // We're done - just use the implementation from Eigen 34 | 35 | #else // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // 36 | // Eigen version is older than 3.3.4: CompleteOrthogonalDecomposition is not defined 37 | // Implement much of the API for CompleteOrthogonalDecomposition, but use the same back-end as 38 | // the original implementation of the SNS-IK solver. 39 | // https://eigen.tuxfamily.org/dox/classEigen_1_1CompleteOrthogonalDecomposition.html 40 | 41 | #include "sns_ik_math_utils.hpp" 42 | #include 43 | 44 | namespace sns_ik { 45 | 46 | static const double DEFAULT_THRESHOLD = 1e-8; 47 | 48 | SnsLinearSolver::SnsLinearSolver() 49 | : info_(Eigen::Success), rank_(0) 50 | { 51 | setThreshold(Eigen::Default); 52 | } 53 | 54 | SnsLinearSolver::SnsLinearSolver(int n, int m) 55 | : info_(Eigen::Success), A_(n, m), invA_(m, n), rank_(0) 56 | { 57 | setThreshold(Eigen::Default); 58 | } 59 | 60 | SnsLinearSolver::SnsLinearSolver(const Eigen::MatrixXd& A) 61 | : SnsLinearSolver(A.rows(), A.cols()) 62 | { 63 | setThreshold(Eigen::Default); compute(A); 64 | } 65 | 66 | Eigen::MatrixXd SnsLinearSolver::solve(const Eigen::MatrixXd& b) 67 | { 68 | static bool PRINT_WARNING = true; 69 | if (PRINT_WARNING) { 70 | ROS_WARN("Using legacy code. Upgrade to at least Eigen 3.3.4 for improved linear solver."); 71 | PRINT_WARNING = false; // only print the warning once. 72 | } 73 | return invA_ * b; 74 | } 75 | 76 | void SnsLinearSolver::compute(const Eigen::MatrixXd& A) 77 | { 78 | A_ = A; 79 | if (!sns_ik::pseudoInverse(A_, tol_, &invA_, &rank_)) { 80 | info_ = Eigen::InvalidInput; 81 | } 82 | } 83 | 84 | void SnsLinearSolver::setThreshold(Eigen::Default_t tol) { 85 | setThreshold(DEFAULT_THRESHOLD); 86 | } 87 | 88 | } // namespace sns_ik 89 | 90 | #endif // EIGEN_VERSION_AT_LEAST(3,3,4) //- - - - - - - - - - - - - - - - - - - - - - - - - - // 91 | -------------------------------------------------------------------------------- /sns_ik_lib/utilities/sns_linear_solver.hpp: -------------------------------------------------------------------------------- 1 | /** @file sns_linear_solver.hpp 2 | * 3 | * @brief Linear Solver, used by the SNS-IK velocity IK solvers. 4 | * @author: Matthew Kelly 5 | */ 6 | 7 | /** 8 | * Copyright 2018 Rethink Robotics 9 | * 10 | * Licensed under the Apache License, Version 2.0 (the "License"); 11 | * you may not use this file except in compliance with the License. 12 | * You may obtain a copy of the License at 13 | * http://www.apache.org/licenses/LICENSE-2.0 14 | * 15 | * Unless required by applicable law or agreed to in writing, software 16 | * distributed under the License is distributed on an "AS IS" BASIS, 17 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | * See the License for the specific language governing permissions and 19 | * limitations under the License. 20 | */ 21 | 22 | /* 23 | * 24 | * This class is used to solve linear systems. It is a wrapper for two different internal solvers: 25 | * --> psuedo-inverse solver: included for legacy support on Eigen 3.2.0 26 | * --> direct linear solver: prefered solver when available. Requries Eigen 3.3.4 27 | */ 28 | 29 | #ifndef SNS_IK_LIB__SNS_LINEAR_SOLVER_H_ 30 | #define SNS_IK_LIB__SNS_LINEAR_SOLVER_H_ 31 | 32 | #include 33 | 34 | namespace sns_ik { 35 | 36 | #if EIGEN_VERSION_AT_LEAST(3,3,4) // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // 37 | // Eigen version is newer than 3.3.4: CompleteOrthogonalDecomposition is defined 38 | typedef Eigen::CompleteOrthogonalDecomposition SnsLinearSolver; 39 | 40 | #else // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - // 41 | // Eigen version is older than 3.3.4: CompleteOrthogonalDecomposition is not defined 42 | // Implement much of the API for CompleteOrthogonalDecomposition, but use the same back-end as 43 | // the original implementation of the SNS-IK solver. 44 | // https://eigen.tuxfamily.org/dox/classEigen_1_1CompleteOrthogonalDecomposition.html 45 | 46 | class SnsLinearSolver { 47 | 48 | public: 49 | 50 | /* 51 | * Create a default linear solver 52 | */ 53 | SnsLinearSolver(); 54 | 55 | /* 56 | * Create a default linear solver with memory preallocation 57 | */ 58 | SnsLinearSolver(int n, int m); 59 | 60 | /* 61 | * Create a linear solver for the matrix A*x = b 62 | * @param A: matrix of interest 63 | */ 64 | SnsLinearSolver(const Eigen::MatrixXd& A); 65 | 66 | /* 67 | * Set and decompose the matrix in the linear system. 68 | */ 69 | void compute(const Eigen::MatrixXd& A); 70 | 71 | /* 72 | * Find x to minimize: ||A*x - b||^2 73 | * @param b: right hand side of the linear system 74 | * @return: x = solution to the optimization problem 75 | */ 76 | Eigen::MatrixXd solve(const Eigen::MatrixXd& b); 77 | 78 | /* 79 | * @return: status of the solver 80 | */ 81 | Eigen::ComputationInfo info() const { return info_; }; 82 | 83 | /* 84 | * Set the threshold that is used for computing rank and pseudoinverse 85 | * @param tol: threshold used for decomposing matrix and computing rank 86 | */ 87 | void setThreshold(Eigen::Default_t tol); 88 | 89 | /* 90 | * Set the threshold that is used for computing rank and pseudoinverse 91 | * @param tol: threshold used for decomposing matrix and computing rank 92 | */ 93 | void setThreshold(double tol) { tol_ = tol; }; 94 | 95 | /* 96 | * @return: the rank of the matrix A 97 | */ 98 | unsigned int rank() const { return rank_; }; 99 | 100 | private: 101 | 102 | // status of the most recent solve operation 103 | Eigen::ComputationInfo info_; 104 | 105 | // the "A" matrix in A*x = b 106 | Eigen::MatrixXd A_; 107 | 108 | // the pseudo-inverse of "A" 109 | Eigen::MatrixXd invA_; 110 | 111 | // tolerance that is used for checking for non-trivial singular values 112 | double tol_; 113 | 114 | // rank of "A" 115 | int rank_; 116 | 117 | }; 118 | 119 | #endif // EIGEN_VERSION_AT_LEAST(3,3,4) //- - - - - - - - - - - - - - - - - - - - - - - - - - // 120 | 121 | } // namespace sns_ik 122 | 123 | #endif // SNS_IK_LIB__SNS_LINEAR_SOLVER_H_ 124 | --------------------------------------------------------------------------------