diff --git a/CMakeLists.txt b/CMakeLists.txt index 77c1702f4..08c8f0512 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,6 +108,8 @@ if (${CMAKE_SYSTEM_NAME} MATCHES Linux) message("-- *** FOUND ROS ON THIS MACHINE, ENABLING SUPPORT FOR AMBF_ROS MODULES") add_subdirectory(${PROJECT_SOURCE_DIR}/ambf_ros_modules) add_subdirectory(${PROJECT_SOURCE_DIR}/external/tf_function) + # AMBF Utilities + add_subdirectory(${PROJECT_SOURCE_DIR}/ambf_utilities) # Call find catkin again, with REQUIRED this time, it's okay to write over previously # imported catkin variables find_package(catkin QUIET COMPONENTS ambf_comm ambf_msgs dvrk_arm razer_hydra cv_bridge image_transport) diff --git a/GithubUpdateScript.sh b/GithubUpdateScript.sh deleted file mode 100755 index 4cd6adf84..000000000 --- a/GithubUpdateScript.sh +++ /dev/null @@ -1,27 +0,0 @@ -# The following script should automate the github update process -# If the .sh file is not executable, do the following: -# -# sudo chmod 700 GithubUpdateScript.sh -# -# Enjoy! - -printf "\n Start Github Update...\n" - -git fetch upstream -git pull upstream master -git push -git status - -printf "\n See what to add and do it on a new terminal!\n" -gnome-terminal -printf " Press any key to continue (ctrl-C to quit)" -read -n 1 -s -r -p "..." - -git status -printf "\n Now do they all look good? Press any key to continue (ctrl-C to quit)" -read -n 1 -s -r -p "..." - -git commit -git push - -printf "\n All done. Great work!\n" \ No newline at end of file diff --git a/README.md b/README.md deleted file mode 100644 index bd5790d59..000000000 --- a/README.md +++ /dev/null @@ -1,246 +0,0 @@ -# Asynchronous Multi-Body Framework (AMBF) -### 1. Author: [Adnan Munawar](https://github.com/adnanmunawar) (amunawar@wpi.edu) - -#### Build Status - -![ambf-1.0](https://github.com/WPI-AIM/ambf/workflows/ambf-1.0/badge.svg?branch=ambf-1.0) - -#### Contributors: -1. [Melody Su](https://github.com/melodysu83) (Email: --) - - -### 2. [Wiki](https://github.com/WPI-AIM/ambf/wiki): -Please checkout the [Wiki](https://github.com/WPI-AIM/ambf/wiki) for indepth details about AMBF, it's components, examples and concepts. You can also checkout the video below for a brief run down of some of the features of AMBF - -[![AMBF Simulator](https://img.youtube.com/vi/9REJVK1mwEU/maxresdefault.jpg)](https://www.youtube.com/watch?v=9REJVK1mwEU&t=0s) - - -### 3. Description: -This multi-body framework offers real-time dynamic simulation of multi-bodies (robots, free -bodies and multi-link puzzles) coupled with real-time haptic interaction via several haptic devices -(CHAI-3D) (including dVRK Manipulators and Razer Hydras). It also provides a python client for training NN and -RL Agents on real-time data with simulation in the loop. This framework is built around several -external tools that include an extended version of CHAI-3D (developed along-side AMBF), BULLET-Physics, Open-GL, GLFW, yaml-cpp, pyyaml and Eigen to name a few. Each external library has it's own license that can be found in the corresponding subfolder. - -### 4. Usage: -#### 4.1 Tested Platforms: -AMBF has been tested on **Ubuntu 16.04** and **Ubuntu 18.04**. We need a few extra steps on **Ubuntu 14.04**, please create an issue if you would like to get instructions for that. - -Even though it is recommended to use Linux for the full feature set of AMBF Simulator using ROS, AMBF has been tested on **MacOS Maverick** and **MacOS Mojave** without ROS support. - - -#### 4.2 Building: -On Linux machines, you might need to install the `libasound2-dev` package and external libraries dependencies. - -``` -sudo apt install libasound2-dev libgl1-mesa-dev xorg-dev -``` - -Boost libraires ship with Ubuntu systems, but on Mac OS, you might need to install them explicitly. - -For this purpose, on **Mac OS**, if you don't have Boost - -1. Install **Xcode** from App Store -2. Install **command line tools** by running in terminal -`xcode-select --install` -3. Install **Homebrew** view running this in terminal -`/usr/bin/ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)"` - -4. Install **boost** by running the following in the terminal -`brew install boost` - - -To build the framework (Linux and Mac-OS) and get various addon scripts (blender, yaml <-> urdf): -``` -cd ~ -git clone https://github.com/WPI-AIM/ambf.git -cd ambf && mkdir build -git submodule update --init --recursive -cd build -cmake .. -make -``` - -On Linux systems, please source the correct folder to achieve system wide availability of AMBF ROS modules. -While in the build folder, you can run: - -`source ./devel/setup.bash` - -You can also permanently add the install location in your .bashrc with the following command: - -`echo "source ~/ambf/build/devel/setup.bash" >> ~/.bashrc` - -#### 4.3 Running the Simulator: -Having succesfully completed the steps above running is Simulator is easy. Depending -on what OS you're using simply follow the commands below: - -``` -cd ~/ambf/bin/ -./ambf_simulator -``` - -#### 4.4 Launching Specific Multibodies: -There are two ways to launch multibodies: -1. Using the integer index of the filename specified in the launch file -2. Providing the explicit filename(s). - -##### 4.4.1 Using the Integer Index in the launch file -The -l command line argument can be used to launch a specific multibody at launch using indexing. The multibodies are defined in [ambf_models/descriptions/launch.yaml](https://github.com/WPI-AIM/ambf/blob/master/ambf_models/descriptions/launch.yaml) and are commented with indices for ease of identification. As a default behaviour, launching the simulator without the `-l` command line argument loads the first multi-body defined in the `launch.yaml`. To launch a specific multi-body you can use the `-l` flag with the integer index of the multi-body as follows: - -``` -cd ~/ambf/bin/ -./ambf_simulator -l 4 -``` -This command will launch the 4th body defined in the `launch.yaml` file. To launch multiple multi-bodies, you can use a comma separated list (without spaces in between) of integers indices e.g.`./ambf_simulator -l 1,6,10`. This in turn would load the multi-bodies defined at 1, 6 and the 10th index in the `launch.yaml` file. - -##### 4.4.2 Providing the fully qualified filename -The second option is to use the `-a` flag. For example, if one has an AMBF description file in the home directory `/users/potato/tests/robot.yaml`, this file can be launched directly as follows - -``` -cd ~/ambf/bin/ -./ambf_simulator -a /users/potato/tests/robot.yaml -``` - -Similarly, as it the case with the `-l` flag, multiple filenames can be launch by comma separated values. E.g. - - -``` -cd ~/ambf/bin/ -./ambf_simulator -a /users/potato/tests/robot.yaml,/users/potato/tests/car.yaml -``` - -Lastly, the `-l` and `-a` flags can be used together to launch some files based on the index and some based on the filenames. - -##### 4.4.3 Note: -The AMBF Simulator uses the yaml file located in `ambf/ambf_models/descriptions/launch.yaml` to -load robot/multi-body models, haptic device end-effectors and the world. You can see the contents -of this file by launching it in your favourite text editor. For an initial overview, the most important thing is the field `multibody configs:`. The uncommented config file(s) will be launced at startup. Multiple -config files can be launched at the same time by uncommenting them. You can play around with a few config -files to see how they work. - -### 5 Interacting with the Robots/Multi-Bodies in the Simulator: -There are multiple way of interacting with the bodies in simulator. If you are using Linux, the -provided Python client offers a convenient user interface and robust API. - -For full feature set of the AMBF Simulator, it is advised that you install it on Linux (Ubuntu) 16, 17 or 18. Other variants might be supported but have not yet been tested. - -#### 5.1 The AMBF Python Client -This simplest way to interact with simulated bodies, robots/multi-bodies, kinematic and visual objects in the AMBF simulator is by using the high-speed Asynchronous Communication that is implemented via ROS-topics in the AMBF Framework Library. One can use either C++ or Python for this purpose. For ease of interaction we provide a convenient Python Client which can be used as follows: - -## -Start the AMBF Simulator with your choice of Multi-Body config file that (set in the `multi bodies` field in the `ambf_models/launch.yaml` file). - -In your python application - -```python -# Import the Client from ambf_comm package -# You might have to do: pip install gym -from ambf_client import Client -import time - -# Create a instance of the client -_client = Client() - -# Connect the client which in turn creates callable objects from ROS topics -# and initiates a shared pool of threads for bidrectional communication -_client.connect() - -# You can print the names of objects found -print(_client.get_obj_names()) - -# Lets say from the list of printed names, we want to get the -# handle to an object names "Torus" -torus_obj = _client.get_obj_handle('Torus') - -# Now you can use the torus_obj to set and get its position, rotation, -# Pose etc. If the object has joints, you can also control them -# in either position control mode or open loop effort mode. You can even mix and -# match the joints commands -torus_obj.set_pos(0, 0, 0) # Set the XYZ Pos in obj's World frame -torus_obj.set_rpy(1.5, 0.7, .0) # Set the Fixed RPY in World frame -time.sleep(5) # Sleep for a while to see the effect of the command before moving on - -# Other methods to control the obj position include -# torus_obj.set_pose(pose_cmd) # Where pose_cmd is of type Geometry_msgs/Pose -# torus_obj.set_rot(quaterion) # Where quaternion is a list in the order of [qx, qy, qz, qw] -# Finally all the position control params can be controlled in a single method call -# torus_obj.pose_command(px, py, pz, roll, pitch, yaw, *jnt_cmds) - -# We can just as easily get the pose information of the obj -cur_pos = torus_obj.get_pos() # xyx position in World frame -cur_rot = torus_obj.get_rot() # Quaternion in World frame -cur_rpy = torus_obj.get_rpy() # Fixed RPY in World frame - -# Similarly you can directly control the wrench acting on the obj by -# The key difference is that it's the user's job to update the forces -# and torques in a loop otherwise the wrench in cleared after an internal -# watchdog timer expires if a new command is not set. This is for safety -# reasons where a user shouldn't set a wrench and then forget about it. -for i in range(0, 5000): - torus_obj.set_force(5, -5, 10) # Set the force in the World frame - torus_obj.set_torque(0, 0, 0.8) # Set the torque in the World frame - time.sleep(0.001) # Sleep for a while to see the effect of the command before moving on - -# Similar to the pose_command, one can assign the force in a single method call -# torus_obj.wrench_command(fx, fy, fz, nx, ny, nz) in the World frame - -# We can get the number of children and joints connected to this body as -num_joints = torus_obj.get_num_joints() # Get the number of joints of this object -children_names = torus_obj.get_children_names() # Get a list of children names belonging to this obj - -print(num_joints) -print(children_names) - -# If the obj has some joints, we can control them as follows -if num_joints > 1: - torus_obj.set_joint_pos(0, 0.5) # The the joints at idx 0 to 0.5 Radian - torus_obj.set_joint_effort(1, 5) # Set the effort of joint at idx 1 to 5 Nm - time.sleep(2) # Sleep for a while to see the effect of the command before moving on - - -# Lastly to cleanup -_client.clean_up() -``` - -### 6 Raven and DVRK Kinematics Controller Client -See [here](/ambf_controller/README.md) for more information. - -### 7 AMBF Network Setup: -In order to subscribe and publish data using AMBF over multiple machines, the following steps would need to be followed: -1. Check the connectivity between the machines (example: using ssh and ping) -2. Edit the `/etc/hosts` and add the hostnames of the machines, so that the machines can find each other over the network (example: similar to [Adding host name to /etc/hosts](https://www.howtogeek.com/howto/27350/beginner-geek-how-to-edit-your-hosts-file/)) -3. Set the ROS environment variable in local machine to the host using `export ROS_MASTER_URI=http://hostIPaddress:11311` (ex: export ROS_MASTER_URI=http://112.115.256.121:11311) -4. Now you should be able to send and receive ROS messages over the two machines and control AMBF. -5. If you face any firewall issues or if you are unable to receive/publish any ROS topics over the two machines, follow the next step. -6. Open a terminal and type the command: `sudo apt-get install gufw` -7. Next type `sudo gufw` (type the password when prompted) and ensure both the Incoming and Outgoing traffic is allowed. - -### 8 Docker -In order to use the docker file, follow the instructions [here](https://docs.docker.com/install/) to install docker on your system. To run the file: - -```bash -cd ~/ -git clone https://github.com/WPI-AIM/ambf.git && cd ambf -sudo service docker start -docker build --rm -f "Dockerfile" -t ambf:latest "." -docker run --rm -it ambf:latest -cd /ambf/bin/lin-x86_64/ -./ambf_simulator -g off -``` - -## Citation -If this work is helpful for your research, please use the following reference for citation: -``` -@INPROCEEDINGS{8968568, -author={A. {Munawar} and Y. {Wang} and R. {Gondokaryono} and G. S. {Fischer}}, -booktitle={2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, -title={A Real-Time Dynamic Simulator and an Associated Front-End Representation Format for Simulating Complex Robots and Environments}, -year={2019}, -volume={}, -number={}, -pages={1875-1882}, -keywords={}, -doi={10.1109/IROS40897.2019.8968568}, -ISSN={2153-0858}, -month={Nov},} -``` diff --git a/__init__.py b/__init__.py index 907cee236..55e715dfd 100644 --- a/__init__.py +++ b/__init__.py @@ -1 +1,6 @@ from .external.tf_function import transformations +from .ambf_utilities.tf_utils.scripts.vector import Vector, dot +from .ambf_utilities.tf_utils.scripts.frame import Frame +from .ambf_utilities.tf_utils.scripts.rotation import Rotation +from .ambf_utilities.tf_utils.scripts.twist import Twist +from .ambf_utilities.tf_utils.scripts.wrench import Wrench \ No newline at end of file diff --git a/ambf_controller/dvrk/scripts/utilities.py b/ambf_controller/dvrk/scripts/utilities.py index 55696cf18..f347bd95b 100644 --- a/ambf_controller/dvrk/scripts/utilities.py +++ b/ambf_controller/dvrk/scripts/utilities.py @@ -1,8 +1,10 @@ #!/usr/bin/env python import sys -if sys.version_info < (3,0): - from PyKDL import Vector, Rotation, Frame, dot +import PyKDL as p +from frame import Frame +from vector import Vector, dot +from rotation import Rotation import numpy as np import math @@ -10,30 +12,61 @@ PI_2 = np.pi/2 -if sys.version_info < (3,0): - # The up vector is useful to define angles > PI. Since otherwise - # this method will only report angles <= PI. - def get_angle(vec_a, vec_b, up_vector=None): - vec_a.Normalize() - vec_b.Normalize() - cross_ab = vec_a * vec_b - vdot = dot(vec_a, vec_b) - # print('VDOT', vdot, vec_a, vec_b) - # Check if the vectors are in the same direction - if 1.0 - vdot < 0.000001: - angle = 0.0 - # Or in the opposite direction - elif 1.0 + vdot < 0.000001: - angle = np.pi - else: - angle = math.acos(vdot) - - if up_vector is not None: - same_dir = np.sign(dot(cross_ab, up_vector)) - if same_dir < 0.0: - angle = -angle - - return angle +# The up vector is useful to define angles > PI. Since otherwise +# this method will only report angles <= PI. +def py_kdl_get_angle(vec_a, vec_b, up_vector=None): + vec_a.Normalize() + vec_b.Normalize() + cross_ab = vec_a * vec_b + vdot = p.dot(vec_a, vec_b) + # print('VDOT', vdot, vec_a, vec_b) + # Check if the vectors are in the same direction + if 1.0 - vdot < 0.000001: + angle = 0.0 + # Or in the opposite direction + elif 1.0 + vdot < 0.000001: + angle = np.pi + else: + angle = math.acos(vdot) + + if up_vector is not None: + same_dir = np.sign(dot(cross_ab, up_vector)) + if same_dir < 0.0: + angle = -angle + + return angle + +# The up vector is useful to define angles > PI. Since otherwise +# this method will only report angles <= PI. +def tf_utils_get_angle(vec_a, vec_b, up_vector=None): + vec_a.Normalize() + vec_b.Normalize() + cross_ab = vec_a * vec_b + vdot = dot(vec_a, vec_b) + # print('VDOT', vdot, vec_a, vec_b) + # Check if the vectors are in the same direction + if 1.0 - vdot < 0.000001: + angle = 0.0 + # Or in the opposite direction + elif 1.0 + vdot < 0.000001: + angle = np.pi + else: + angle = math.acos(vdot) + + if up_vector is not None: + same_dir = np.sign(dot(cross_ab, up_vector)) + if same_dir < 0.0: + angle = -angle + + return angle + +def get_angle(vec_a, vec_b, up_vector=None): + # pykdl_angle = py_kdl_get_angle(vec_a, vec_b, up_vector) + tf_utils_angle = tf_utils_get_angle(vec_a, vec_b, up_vector) + + # assert pykdl_angle == tf_utils_angle, 'Angles do not match' + + return tf_utils_angle def round_mat(mat, rows, cols, precision=4): @@ -68,14 +101,34 @@ def convert_frame_to_mat(frame): return np_mat -if sys.version_info < (3,0): - def convert_mat_to_frame(mat): - frame = Frame(Rotation.RPY(0, 0, 0), Vector(0, 0, 0)) - for i in range(3): - for j in range(3): - frame[(i, j)] = mat[i, j] +def pykdl_convert_mat_to_frame(mat): + frame = p.Frame(p.Rotation.RPY(0, 0, 0), p.Vector(0, 0, 0)) + for i in range(3): + for j in range(3): + frame[(i, j)] = mat[i, j] + + for i in range(3): + frame.p[i] = mat[i, 3] + + return frame + +def tf_utils_convert_mat_to_frame(mat): + frame = Frame(Rotation.RPY(0, 0, 0), Vector(0, 0, 0)) + for i in range(3): + for j in range(3): + frame.M[i, j] = mat[i, j] + + for i in range(3): + frame.p[i] = mat[i, 3] + + return frame + +def convert_mat_to_frame(mat): + pykdl_frame = pykdl_convert_mat_to_frame(mat) + tf_utils_frame = tf_utils_convert_mat_to_frame(mat) - for i in range(3): - frame.p[i] = mat[i, 3] + # a = np.asarray(pykdl_frame.M) + # assert np.all(a, tf_utils_frame.M), 'Rotations do not match' + # assert np.all(pykdl_frame.p, tf_utils_frame.p), 'Points do not match' - return frame \ No newline at end of file + return tf_utils_frame \ No newline at end of file diff --git a/ambf_utilities/CMakeLists.txt b/ambf_utilities/CMakeLists.txt new file mode 100644 index 000000000..180c33cb4 --- /dev/null +++ b/ambf_utilities/CMakeLists.txt @@ -0,0 +1,40 @@ +# Software License Agreement (BSD License) +# Copyright (c) 2020, AMBF. +# (https://github.com/WPI-AIM/ambf) +# +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# +# * Neither the name of authors nor the names of its contributors may +# be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# $Author: Dhruv Kool Rajamani, Adnan Munawar $ +# $Date: $ +# $Rev: $ + +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/tf_utils) diff --git a/ambf_utilities/__init__.py b/ambf_utilities/__init__.py new file mode 100644 index 000000000..5a9819b9c --- /dev/null +++ b/ambf_utilities/__init__.py @@ -0,0 +1,6 @@ +from .tf_utils.scripts.vector import Vector, dot +from .tf_utils.scripts.frame import Frame +from .tf_utils.scripts.rotation import Rotation +from .tf_utils.scripts.twist import Twist +from .tf_utils.scripts.wrench import Wrench + diff --git a/ambf_utilities/tf_utils/CMakeLists.txt b/ambf_utilities/tf_utils/CMakeLists.txt new file mode 100644 index 000000000..be230c731 --- /dev/null +++ b/ambf_utilities/tf_utils/CMakeLists.txt @@ -0,0 +1,201 @@ +cmake_minimum_required(VERSION 2.8.3) +project(tf_utils) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# LIBRARIES tf_function transformations +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/tf_function.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/tf_function_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts ( etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/transformations +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_tf_function.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ambf_utilities/tf_utils/__init__.py b/ambf_utilities/tf_utils/__init__.py new file mode 100644 index 000000000..1ad812228 --- /dev/null +++ b/ambf_utilities/tf_utils/__init__.py @@ -0,0 +1,5 @@ +from .scripts.vector import Vector, dot +from .scripts.frame import Frame +from .scripts.rotation import Rotation +from .scripts.twist import Twist +from .scripts.wrench import Wrench diff --git a/ambf_utilities/tf_utils/package.xml b/ambf_utilities/tf_utils/package.xml new file mode 100644 index 000000000..a68c7386c --- /dev/null +++ b/ambf_utilities/tf_utils/package.xml @@ -0,0 +1,59 @@ + + + tf_utils + 0.0.1 + The tf_utils package + + + + + Dhruv Kool Rajamani + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/ambf_utilities/tf_utils/scripts/__init__.py b/ambf_utilities/tf_utils/scripts/__init__.py new file mode 100644 index 000000000..f10f1badc --- /dev/null +++ b/ambf_utilities/tf_utils/scripts/__init__.py @@ -0,0 +1,5 @@ +from .vector import Vector, dot +from .frame import Frame +from .rotation import Rotation +from .twist import Twist +from .wrench import Wrench diff --git a/ambf_utilities/tf_utils/scripts/frame.py b/ambf_utilities/tf_utils/scripts/frame.py new file mode 100644 index 000000000..5678daf01 --- /dev/null +++ b/ambf_utilities/tf_utils/scripts/frame.py @@ -0,0 +1,205 @@ +""" This class will generate a Frame in 3D space to mimic Frame in pyKDL""" + +import numpy as np +from numpy import pi, sin, cos, tan, arctan2 +from numpy.core.numeric import identity +from vector import Vector +from twist import Twist +from wrench import Wrench +from rotation import Rotation +from warnings import warn +import sys + + +class Frame(object): + + @staticmethod + def make_Frame(HTM): + """ + Return a frame from a 4x4 Homogenous transformaiton matrix + + @param 4x4mat Homogenous TF Matrix + + @return Frame + """ + + frame = Frame(HTM[:3, :3], HTM[0:3, 3]) + return frame + + @staticmethod + def make_HTM(f): + """ + Return a 4x4 Homogenous Transformation Matrix from a Frame + + @param Frame f + + @return 4x4mat Homogenous TF Matrix + """ + + mat_44 = np.ndarray((4, 4)) + mat_44[:3, :3] = f.M + mat_44[3:, :4] = np.array([0, 0, 0, 1]) + mat_44[:3, 3] = f.p + + return mat_44 + + @staticmethod + def Identity(): + """ + Constructs an identity frame + + @return Frame + """ + + return Frame() + + @staticmethod + def DH(a, alpha, d, theta): + """ + Constructs a transformationmatrix T_link(i-1)_link(i) with the Denavit-Hartenberg + convention as described in the original publictation: Denavit, J. and Hartenberg, + R. S., A kinematic notation for lower-pair mechanisms based on matrices, + ASME Journal of Applied Mechanics, 23:215-221, 1955. + + @param float a + @param float alpha (radians) + @param float d + @param float theta (radians) + + @return Frame + """ + + ct = cos(theta) + st = sin(theta) + sa = sin(alpha) + ca = cos(alpha) + return Frame( + Rotation(ct, + -st * ca, + st * sa, + st, + ct * ca, + -ct * sa, + 0, + sa, + ca), + Vector(a * ct, + a * st, + d) + ) + + @staticmethod + def DH_Craig1989(a, alpha, d, theta): + """ + Constructs a transformationmatrix T_link(i-1)_link(i) with the Denavit-Hartenberg + convention as described in the Craigs book: Craig, J. J.,Introduction to Robotics: + Mechanics and Control, Addison-Wesley, isbn:0-201-10326-5, 1986. + + @param float a + @param float alpha (radians) + @param float d + @param float theta (radians) + + @return Frame + """ + + ct = cos(theta) + st = sin(theta) + sa = sin(alpha) + ca = cos(alpha) + return Frame( + Rotation(ct, + -st, + 0, + st * ca, + ct * ca, + -sa, + st * sa, + ct * sa, + ca), + Vector(a, + -sa * d, + ca * d) + ) + + @staticmethod + def AddDelta(f, t, d): + """ + NOT IMPLEMENTED + Constructs a frame that is obtained by: starting from frame f, apply twist t, during time d + + @param Frame f + @param Twist t + @param float d + + @return Frame + """ + + return + + def __init__(self, rot=None, pos=None): + """ + Construct an an object of frame + + @param Rotation rot + @param Vector pos + """ + super(Frame, self).__init__() + + self.M = None + self.p = None + + if rot is not None: + self.M = Rotation(rot) + else: + self.M = Rotation() + + if pos is not None: + self.p = Vector(pos) + else: + self.p = Vector() + + return + + def Integrate(self, twist, frequency): + """ + This frame is integrated into an updated frame with sample frequence, using first order integration + NOT IMPLEMENTED + """ + + return + + def Inverse(self): + """ + Returns the inverse of the frame + + @return Frame + """ + + self.M.SetInverse() + self.p = -self.M * self.p + + return self + + def __mul__(self, f): + + this_mat_44 = Frame.make_HTM(self) + f_mat_44 = Frame.make_HTM(f) + + mul = np.matmul(this_mat_44, f_mat_44) + + frame = Frame.make_Frame(mul) + + return frame + + +def test(): + f1 = Frame() + f2 = Frame() + f = f1 * f2 + print(f.p) + print(f.M) + + +if __name__ == '__main__': + test() diff --git a/ambf_utilities/tf_utils/scripts/rotation.py b/ambf_utilities/tf_utils/scripts/rotation.py new file mode 100644 index 000000000..a9c48f394 --- /dev/null +++ b/ambf_utilities/tf_utils/scripts/rotation.py @@ -0,0 +1,605 @@ +""" This class will generate a Rotation in 3D to mimic Rotation in pyKDL""" + +import numpy as np +from numpy import pi, sin, cos, tan, arctan2 +from vector import Vector +from twist import Twist +from wrench import Wrench +from warnings import warn +import sys + +class Rotation(np.ndarray): + + X_AX = Vector(1., 0., 0.) + Y_AX = Vector(0., 1., 0.) + Z_AX = Vector(0., 0., 1.) + + epsilon = 1e-12 + + @staticmethod + def RPY(roll=0., pitch=0., yaw=0.): + """ + Get the rotation matrix for a rpy + + @param float roll + @param float pitch + @param float yaw + + @return Rotation + """ + + ca1 = cos(yaw) + sa1 = sin(yaw) + cb1 = cos(pitch) + sb1 = sin(pitch) + cc1 = cos(roll) + sc1 = sin(roll) + + return Rotation(ca1*cb1, ca1*sb1*sc1 - sa1*cc1, ca1*sb1*cc1 + sa1*sc1, + sa1*cb1, sa1*sb1*sc1 + ca1*cc1, sa1*sb1*cc1 - ca1*sc1, + -sb1, cb1*sc1, cb1*cc1) + + @staticmethod + def EulerZYZ(z1=0., y=0., z2=0.): + """ + Get the rotation matrix for a ZYZ rotation + + @param float z1 + @param float y + @param float z2 + + @return Rotation + """ + + sa = sin(z1) + ca = cos(z1) + sb = sin(y) + cb = cos(y) + sg = sin(z2) + cg = cos(z2) + + return Rotation(ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb, + sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb, + -sb*cg, sb*sg, cb + ) + + @staticmethod + def EulerZYX(x=0., y=0., z=0.): + """ + Get the rotation matrix for a ZYX rotation + + @param float z + @param float y + @param float x + + @return Rotation + """ + + return Rotation.RPY(z, y, x) + + @staticmethod + def is_valid(mat): + """ + Checks if the rotation is a valid rotation matrix. Also checks for + singularities + + @param mat 3x3 + @returns mat + """ + x = Vector(mat[:,0]) + y = Vector(mat[:,1]) + z = Vector(mat[:,2]) + try: + vNorms = Vector(x.Norm(), y.Norm(), z.Norm()) + assert not np.any(vNorms[vNorms > 1]) + assert not np.any(mat[mat > 1]) + except AssertionError as norm_error: + warn("Values are not normalized. Auto normalize will take place.") + x.Normalize() + y.Normalize() + z.Normalize() + + try: + assert not np.array_equal(x,y) + assert not np.array_equal(x,z) + assert not np.array_equal(z,y) + x = Vector(mat[0,:]) + y = Vector(mat[1,:]) + z = Vector(mat[2,:]) + assert not np.array_equal(x,y) + assert not np.array_equal(x,z) + assert not np.array_equal(z,y) + except AssertionError as singular_error: + warn("Rotation is singular, raising exception to exit") + sys.exit() + + mat[:,0] = x + mat[:,1] = y + mat[:,2] = z + + return mat + + @staticmethod + def Rot(axis, angle): + """ + Get the rotation matrix using the axis and angle + + @param Vector axis + @param float angle + + @return Rotation + """ + axis.Normalize() + + ct = cos(angle) + st = sin(angle) + vt = 1-ct + m_vt_0=vt*axis[0] + m_vt_1=vt*axis[1] + m_vt_2=vt*axis[2] + m_st_0=axis[0]*st + m_st_1=axis[1]*st + m_st_2=axis[2]*st + m_vt_0_1=m_vt_0*axis[1] + m_vt_0_2=m_vt_0*axis[2] + m_vt_1_2=m_vt_1*axis[2] + + return Rotation( + ct + m_vt_0*axis[0], + -m_st_2 + m_vt_0_1, + m_st_1 + m_vt_0_2, + m_st_2 + m_vt_0_1, + ct + m_vt_1*axis[1], + -m_st_0 + m_vt_1_2, + -m_st_1 + m_vt_0_2, + m_st_0 + m_vt_1_2, + ct + m_vt_2*axis[2] + ) + + @staticmethod + def Identity(): + """ + Get the Identity rotation matrix + + @return Rotation + """ + + return Rotation() + + @staticmethod + def Quaternion(x, y, z, w): + """ + Get the rotation matrix for a quaternion rotation + + @param float x + @param float y + @param float z + @param float w + + @return Rotation + """ + + x2 = x*x + y2 = y*y + z2 = z*z + w2 = w*w + + return Rotation(w2+x2-y2-z2, 2*x*y-2*w*z, 2*x*z+2*w*y, + 2*x*y+2*w*z, w2-x2+y2-z2, 2*y*z-2*w*x, + 2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2) + + def __new__(cls, *args): + obj = None + if len(args) == 3: + # x, y, z vectors + mat = np.identity(3) + for i in range(3): + for j in range(3): + mat[j,i] = args[i][j] + mat = Rotation.is_valid(mat) + obj = np.asarray(mat).view(cls) + obj.info = "Rotation of type dir_vectors" + elif len(args) == 4: + # x, y, z vectors + mat = np.identity(3) + for i in range(3): + for j in range(3): + mat[j,i] = args[i][j] + mat = Rotation.is_valid(mat) + obj = np.asarray(mat).view(cls) + obj.info = args[3] + elif len(args) == 9: + # Don't know whether pyKDL does it row or column vector based + # Need to circle back during testing + # Doing it the Col vector way for now. + # Xx, Yx, Zx, Xy, Yy, Zy, Xz, Yz, Zz + mat = np.identity(3).flatten() + x = None + y = None + z = None + for i in range(9): + mat[i] = args[i] + mat = mat.reshape(3,3) + mat = Rotation.is_valid(mat) + obj = np.asarray(mat).view(cls) + obj.info = "Rotation of type ele" + elif len(args) == 10: + # Xx, Yx, Zx, Xy, Yy, Zy, Xz, Yz, Zz, info + mat = np.identity(3).flatten() + x = None + y = None + z = None + for i in range(9): + mat[i] = args[i] + mat = mat.reshape(3,3) + mat = Rotation.is_valid(mat) + obj = np.asarray(mat).view(cls) + obj.info = args[9] + elif len(args) == 1: + mat = Rotation.is_valid(args[0]) + obj = np.asarray(mat).view(cls) + obj.info = "Rotation of type Rotation" + else: + # identity + mat = np.identity(3) + obj = np.asarray(mat).view(cls) + obj.info = "Rotation of type Identity" + + return obj + + def __array_finalize__(self, obj): + # see InfoArray.__array_finalize__ for comments + if obj is None: return + self.info = getattr(obj, 'info', None) + + def DoRotX(self, angle, unit='rad'): + """ + Rotate about the x-axis with angle in radians + + @param angle in radians (default) + @param unit can be 'rad' or 'deg' + + @return None + """ + + if unit == 'deg': + angle = float(angle) * (pi / 180) + + self = Rotation.Rot(self.X_AX, angle) + + def DoRotY(self, angle, unit='rad'): + """ + Rotate about the y-axis with angle in radians + + @param angle in radians (default) + @param unit can be 'rad' or 'deg' + + @return None + """ + if unit == 'deg': + angle = float(angle) * (pi / 180) + + self = Rotation.Rot(self.Y_AX, angle) + + def DoRotZ(self, angle, unit='rad'): + """ + Rotate about the z-axis with angle in radians + + @param angle in radians (default) + @param unit can be 'rad' or 'deg' + + @return None + """ + if unit == 'deg': + angle = float(angle) * (pi / 180) + + self = Rotation.Rot(self.Z_AX, angle) + + def GetEulerZYX(self): + """ + EulerZYX constructs a Rotation from the Euler ZYX parameters: + + First rotate around Z with z1, + then around the new Y with y, + then around new X with z2. + Closely related to RPY-convention. + + Invariants: + + EulerZYX(alpha,y,z2) == EulerZYX(alpha +/- PI, PI-y, z2 +/- PI) + (angle + 2*k*PI) + + @return (z, y, x) + """ + + return self.GetRPY() + + def GetEulerZYZ(self): + """ + Returns the euler angles for rotations about the Z1, Y and Z2 axes + + @return (z1, y, z2) + """ + + alpha=0.0 + beta=0.0 + gamma=0.0 + if np.abs(self[2,1]) > 1-epsilon: + gamma=0.0 + if self[2,1] > 0: + beta = 0.0 + alpha = arctan2(self[1,0], self[0,0]) + else: + beta = pi + alpha = arctan2(-self[1,0], -self[0,0]) + else: + alpha = arctan2(self[1,2], self[0,2]) + beta = arctan2(np.sqrt(self[2,0]**2 + self[2,1]**2), self[2,2]) + gamma = arctan2(self[2,1], -self[2,0]) + + return (alpha, beta, gamma) + + def GetQuaterninon(self): + """ + Returns the normalized quaternion that describes the rotation + + @return (x, y, z, w) + """ + + trace = self[0,0] + self[1,1] + self[2,2] + + x = 0. + y = 0. + z = 0. + w = 0. + + if trace > self.epsilon: + s = 0.5 / np.sqrt(trace + 1.0) + w = 0.25 / s + x = ( self[2,1] - self[1,2] ) * s + y = ( self[0,2] - self[2,0] ) * s + z = ( self[1,0] - self[0,1] ) * s + else: + if self[0,0] > self[1,1] and self[0,0] > self[2,2]: + s = 2.0 * np.sqrt( 1.0 + self[0,0] - self[1,1] - self[2,2]) + w = (self[2,1] - self[1,2] ) / s + x = 0.25 * s + y = (self[0,1] + self[1,0] ) / s + z = (self[0,2] + self[2,0] ) / s + elif self[1,1] > self[2,2]: + s = 2.0 * np.sqrt( 1.0 + self[1,1] - self[0,0] - self[2,2]) + w = (self[0,2] - self[2,0] ) / s + x = (self[0,1] + self[1,0] ) / s + y = 0.25 * s + z = (self[1,2] + self[2,1] ) / s + else: + s = 2.0 * np.sqrt( 1.0 + self[2,2] - self[0,0] - self[1,1] ) + w = (self[1,0] - self[0,1] ) / s + x = (self[0,2] + self[2,0] ) / s + y = (self[1,2] + self[2,1] ) / s + z = 0.25 * s + + return (x, y, z, w) + + def GetRPY(self): + """ + Returns the r, p, y rotations around fixed axis that describe this rotation. + First a rotation around the x-axis, then a rotation around the original + y-axis, and finally a rotation around the original z-axis + + @return (r, p, y) + """ + + roll = None + yaw = None + pitch = arctan2(self[2,0], np.sqrt(self[0,0]**2 + self[1,0]**2)) + if np.abs(pitch) > (pi/2.) - self.epsilon: + yaw = arctan2(-self[0,1], self[2,2]) + roll = 0. + else: + roll = arctan2(self[2,1], self[2,2]) + yaw = arctan2(self[1,0], self[0,0]) + + return (roll, pitch, yaw) + + def GetRot(self): + """ + Returns a vector with the direction of the equivalent axis and its norm the angle. + This method returns the axis as a Vector + + @return axis Vector + """ + + (axis, angle) = self.GetRotAngle() + return Vector(axis*angle) + + def GetRotAngle(self): + """ + Returns the rotation angle around the equivalent axis. This method returns the + angle as a double, and the rotation axis as a Vector + + @return (angle, axis) + """ + + angle = 0. + x = 0. + y = 0. + z = 0. + + epsilon = self.epsilon + epsilon2 = self.epsilon*10 # margin to distinguish between 0 and 180 degrees + # optional check that input is pure rotation, 'isRotationMatrix' is defined at: + # http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ + # Can use is_valid instead? + if ((np.abs(self[0,1] - self[1,0]) < epsilon) + and (np.abs(self[0,2] - self[2,0]) < epsilon) + and (np.abs(self[1,2] - self[2,1]) < epsilon)): + # singularity found + # first check for identity matrix which must have +1 for all terms + # in leading diagonal and zero in other terms + if ((np.abs(self[0,1] + self[1,0]) < epsilon2) + and (np.abs(self[0,2] + self[2,0]) < epsilon2) + and (np.abs(self[1,2] + self[2,1]) < epsilon2) + and (np.abs(self[0,0] + self[1,1] + self[2,2]-3) < epsilon2)): + # this singularity is identity matrix so angle = 0, axis is arbitrary + # Choose 0, 0, 1 to pass orocos tests + axis = Vector(0,0,1) + angle = 0.0 + return (axis, angle) + + # otherwise this singularity is angle = 180 + angle = pi + xx = (self[0,0] + 1) / 2 + yy = (self[1,1] + 1) / 2 + zz = (self[2,2] + 1) / 2 + xy = (self[0,1] + self[1,0]) / 4 + xz = (self[0,2] + self[2,0]) / 4 + yz = (self[1,2] + self[2,1]) / 4 + + if ((xx > yy) and (xx > zz)): + # self[0,0] is the largest diagonal term + x = np.sqrt(xx) + y = xy/x + z = xz/x + + elif (yy > zz): + # self[1,1] is the largest diagonal term + y = np.sqrt(yy) + x = xy/y + z = yz/y + + else: + # self[2,2] is the largest diagonal term so base result on this + x = xz/z + y = yz/z + + axis = Vector(x, y, z) + return (axis, angle) # return 180 deg rotation + + # If the matrix is slightly non-orthogonal, `f` may be out of the (-1, +1) range. + # Therefore, clamp it between those values to avoid NaNs. + f = (self[0,0] + self[1,1] + self[2,2] - 1) / 2 + angle = np.arccos(np.max(-1.0, np.min(1.0, f))) + + x = (self[2,1] - self[1,2]) + y = (self[0,2] - self[2,0]) + z = (self[1,0] - self[0,1]) + axis = Vector(x, y, z) + axis.Normalize() + return (axis, angle) + + def Inverse(self): + """ + Returns the inverse rotation (this is also the transpose of the rotation matrix) + + @return Rotation + """ + + return Rotation(np.transpose(self)) + + def SetInverse(self): + """ + Sets the inverse rotation (this is also the transpose of the rotation matrix) + + @return None + """ + + self = self.Inverse() + + return + + def UnitX(self): + """ + Returns the column vector for x + + @return Vector + """ + + return Vector(self[0,0], self[1,0], self[2,0]) + + def UnitY(self): + """ + Returns the column vector for x + + @return Vector + """ + + return Vector(self[0,1], self[1,1], self[2,1]) + + def UnitZ(self): + """ + Returns the column vector for x + + @return Vector + """ + + return Vector(self[0,2], self[1,2], self[2,2]) + + def __mul__(self, arg): + + try: + if isinstance(arg, Vector): + """ + Changes the reference frame of a Vector. The norm of the vector does not change. + """ + return Vector(arg.dot(self)) + + elif isinstance(arg, Twist): + """ + Changes the reference frame of a Twist + """ + + return Twist(arg.rot*self, arg.vel) + + elif isinstance(arg, Wrench): + """ + Changes the reference frame of a Wrench + """ + + return Wrench(arg.force*self, arg.torque*self) + + elif isinstance(arg, Rotation): + """ + Changes the reference frame of a Wrench + """ + + return Rotation(np.matmul(self, arg)) + + else: + raise TypeError("arg is not supported for this multiplication") + except TypeError as error: + sys.exit() + + return None + +def vec_test(): + Vec1 = Vector(1,2,-1) + Vec2 = Vector(2,1,-2) + Vec1.Normalize() + print (Vec1) + print (Vec2) + print (Vec1.dot(Vec2)) + print (Vec1.dot(Vec2)) + print (Vec1 * Vec2) + Vec1 = Vector(Vec2) + print (Vec1) + Vec1 += Vec2 + print (Vec1) + +def test(): + rot = Rotation() + X = rot.Rot(Rotation.X_AX,pi/4) + Y = rot.Rot(Rotation.Y_AX,pi/3) + Z = rot.Rot(Rotation.Z_AX,pi/6) + rot = X*Y*Z + print(rot) + (x, y, z, w) = rot.GetQuaterninon() + print(x, y, z, w) + rot = Rotation.Quaternion(x, y, z, w) + (roll, pitch, yaw) = rot.GetRPY() + rot = Rotation.RPY(roll, pitch, yaw) + (x, y, z, w) = rot.GetQuaterninon() + print(x, y, z, w) + +if __name__ == '__main__': + test() diff --git a/ambf_utilities/tf_utils/scripts/twist.py b/ambf_utilities/tf_utils/scripts/twist.py new file mode 100644 index 000000000..6783ab610 --- /dev/null +++ b/ambf_utilities/tf_utils/scripts/twist.py @@ -0,0 +1,95 @@ +""" This class will generate a Vector in 3D to mimic Vector in pyKDL""" + +import numpy as np +from vector import Vector + + +class Twist(object): + rot = Vector() + vel = Vector() + + def __init__(self, rot=None, vel=None): + + super(Twist, self).__init__() + if rot is not None: + self.rot = rot + + if vel is not None: + self.vel = vel + + def RefPoint(self): + """ + Changes the reference point of the Twist. The vector v_base_AB is expressed + in the same base as the twist The vector v_base_AB is a vector from the old + point to the new point. + + ***NOT IMPLEMENTED YET*** + """ + return + + def ReverseSign(self): + """ + Reverse the sign of rot and vel components + + @return None + """ + + self.rot = np.negative(self.rot) + self.vel = np.negative(self.vel) + + return + + def Zero(self): + """ + Returns a zero Twist + + @return Twist + """ + + self.rot = Vector() + self.vel = Vector() + return self + + def __add__(self, t2): + return Twist.add(self, t2) + + def __sub__(self, t2): + return Twist.sub(self, t2) + + def __repr__(self): + return type(self) + + def __str__(self): + return "rot = {}\nvel = {}".format(self.rot, self.vel) + + @staticmethod + def add(t1, t2): + w = Twist() + + w.rot = t1.rot + t2.rot + w.vel = t1.vel + t2.vel + + return w + + @staticmethod + def sub(t1, t2): + w = Twist() + + w.rot = t1.rot - t2.rot + w.vel = t1.vel - t2.vel + + return w + + +def test(): + t1 = Twist(rot=Vector(1, 2, -1), vel=Vector(2, 1, -2)) + t2 = Twist(rot=Vector(0, 1, -1), vel=Vector(2, 2, 0)) + print(t1) + print(t2) + print(t1 - t2) + t1 += t2 + print(t1) + + +if __name__ == '__main__': + test() diff --git a/ambf_utilities/tf_utils/scripts/vector.py b/ambf_utilities/tf_utils/scripts/vector.py new file mode 100644 index 000000000..caeba73aa --- /dev/null +++ b/ambf_utilities/tf_utils/scripts/vector.py @@ -0,0 +1,214 @@ +""" This class will generate a Vector in 3D to mimic Vector in pyKDL""" + +import numpy as np + +class Vector(np.ndarray): + + def __new__(cls, *args): + obj = None + if len(args) == 1: + # Vector + input_array = args[0] + obj = np.asarray(input_array).view(cls) + obj.info = "Vector of type Vector/array" + elif len(args) == 2: + # Vector, info + input_array = args[0] + obj = np.asarray(input_array).view(cls) + obj.info = "Vector of type Vector/array" + obj.info = args[1] + elif len(args) == 3: + # x, y, z + x = args[0] + y = args[1] + z = args[2] + input_array = np.array([x, y, z]) + obj = np.asarray(input_array).view(cls) + obj.info = "Vector of type x, y, z" + elif len(args) == 4: + # x, y, z, info + x = args[0] + y = args[1] + z = args[2] + input_array = np.array([x, y, z]) + obj = np.asarray(input_array).view(cls) + obj.info = args[3] + else: + # zeros + input_array = np.zeros((3,)) + obj = np.asarray(input_array).view(cls) + obj.info = "Vector of Zeros" + + return obj + + def __array_finalize__(self, obj): + # see InfoArray.__array_finalize__ for comments + if obj is None: return + self.info = getattr(obj, 'info', None) + + def Norm(self): + """ + Return the norm of the vector + + @return float + """ + + return np.linalg.norm(self) + + def Normalize(self): + """ + Normalize the vector in place, and return the norm of the vector + + @return float + """ + + self = np.divide(self, np.linalg.norm(self)) + self.info = "Normalized inplace" + return np.linalg.norm(self) + + def ReverseSign(self): + """ + Reverse the sign of the vector + + @return None + """ + + self = np.negative(self) + return + + def Zero(self): + """ + Sets all elements in the vector to zero + + @return None + """ + self = np.zeros((1,3)) + return + + def x(self): + """ + Return the x component of the vector + + @return float + """ + + return self[0] + + def y(self): + """ + Return the y component of the vector + + @return float + """ + + return self[1] + + def z(self): + """ + Return the z component of the vector + + @return float + """ + + return self[2] + + def __repr__(self): + return type(self) + + def __str__(self): + return "[{}, {}, {}]".format(self[0],self[1],self[2]) + + def __mul__(self, v2): + """ + Return the cross product with another vector + + @param Vector + + @return Vector + """ + + if isinstance(v2, Vector): + return Vector.cross(self, v2) + elif isinstance(v2, float): + return np.multiply(v2, self) + + def __sub__(self, args): + v2 = None + output = None + try: + if len(args) == 1: + # Vector + input_array = args[0] + if len(input_array) > 1: + v2 = Vector(input_array) + else: + raise AssertionError + else: + v2 = Vector(args) + + output = np.subtract(self, v2) + except AssertionError as error: + sys.exit() + + return Vector(output) + + def __neg__(self): + """ + Reverse the sign of the vector + + @return Vector + """ + + self.ReverseSign() + return self + + + def dot(self, v2): + """ + Return the dot product with another vector + + @param Vector + + @return Vector + """ + + return Vector._dot(self, v2) + + @staticmethod + def cross(v1, v2): + v = np.cross(v1, v2) + + return Vector(v) + + @staticmethod + def _dot(v1, v2): + v = np.dot(v1[:], v2[:]) + + return v + +def dot(v1, v2): + """ + Return the static method dot of the Vector class + + @param Vector + @param Vector + + @return Vector + """ + return Vector(v1).dot(Vector(v2)) + +def test(): + Vec1 = Vector(1,2,-1) + Vec2 = Vector(2,1,-2) + Vec1.Normalize() + print ("Vector 1: ", Vec1) + print ("Vector 2: ", Vec2) + print ("V1.V2: ", Vec1.dot(Vec2)) + print ("V1 * V2: ", Vec1 * Vec2) + Vec1 = Vector(Vec2) + print ("Vector 1 = Vector 2: ", Vec1) + Vec1 += Vec2 + print ("Vector 2 + Vector 2: ", Vec1) + +if __name__ == '__main__': + test() diff --git a/ambf_utilities/tf_utils/scripts/wrench.py b/ambf_utilities/tf_utils/scripts/wrench.py new file mode 100644 index 000000000..a903f0c80 --- /dev/null +++ b/ambf_utilities/tf_utils/scripts/wrench.py @@ -0,0 +1,94 @@ +""" This class will generate a Vector in 3D to mimic Vector in pyKDL""" + +import numpy as np +from vector import Vector + + +class Wrench(object): + force = Vector() + torque = Vector() + + def __init__(self, force=None, torque=None): + super(Wrench, self).__init__() + if force is not None: + self.force = force + + if torque is not None: + self.torque = torque + + def RefPoint(self): + """ + Changes the reference point of the wrench. The vector v_base_AB is expressed + in the same base as the twist The vector v_base_AB is a vector from the old + point to the new point. + + ***NOT IMPLEMENTED YET*** + """ + return + + def ReverseSign(self): + """ + Reverse the sign of force and torque components + + @return None + """ + + self.force = np.negative(self.force) + self.torque = np.negative(self.torque) + + return + + def Zero(self): + """ + Returns a zero Wrench + + @return Wrench + """ + + self.force = Vector() + self.torque = Vector() + return self + + def __add__(self, w2): + return Wrench.add(self, w2) + + def __sub__(self, w2): + return Wrench.sub(self, w2) + + def __repr__(self): + return type(self) + + def __str__(self): + return "force = {}\ntorque = {}".format(self.force, self.torque) + + @staticmethod + def add(w1, w2): + w = Wrench() + + w.force = w1.force + w2.force + w.torque = w1.torque + w2.torque + + return w + + @staticmethod + def sub(w1, w2): + w = Wrench() + + w.force = w1.force - w2.force + w.torque = w1.torque - w2.torque + + return w + + +def test(): + W1 = Wrench(force=Vector(1, 2, -1), torque=Vector(2, 1, -2)) + W2 = Wrench(force=Vector(0, 1, -1), torque=Vector(2, 2, 0)) + print(W1) + print(W2) + print(W1 - W2) + W1 += W2 + print(W1) + + +if __name__ == '__main__': + test() diff --git a/ambf_utilities/tf_utils/setup.py b/ambf_utilities/tf_utils/setup.py new file mode 100644 index 000000000..479d1ee1f --- /dev/null +++ b/ambf_utilities/tf_utils/setup.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + name='tf_utils', + version='0.0.1', + package_dir={'': 'scripts'}, + packages=['frame', 'vector', 'rotation', 'wrench', 'twist'] +) + +setup(**d) \ No newline at end of file diff --git a/external/tf_function/package.xml b/external/tf_function/package.xml index 66a613b7a..d3f7e7d12 100644 --- a/external/tf_function/package.xml +++ b/external/tf_function/package.xml @@ -56,4 +56,4 @@ - + \ No newline at end of file diff --git a/install/apt-requirements.txt b/install/apt-requirements.txt index 2fc1b6f62..d41d6badc 100644 --- a/install/apt-requirements.txt +++ b/install/apt-requirements.txt @@ -29,6 +29,10 @@ freeglut3 freeglut3-dev libboost-dev cmake +python-psutil +python-future +python3-psutil +python3-future python-nose python-setuptools ros-melodic-tf