From 5aeea3fde6988c2bfe5aa08df4e3691734c20df0 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 22:13:58 -0500 Subject: [PATCH 01/24] Updated readme --- README.md | 31 ++++++++----------------------- 1 file changed, 8 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 2da0cd1..1e0790f 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,10 @@ -# UMRT ROS Template -ROS project repository template for the University of Manitoba Robotics Team. +# UMRT Project Perry Config +Monorepo of ROS packages for configuring the University of Manitoba Robotics Team's software to work with Project Perry. -New projects should be **forked** from this repo (not using this as a template, as that prevents template changes from -trickling down). Each new project must: -1. Fill in missing fields in package.in.xml -2. Fill in project name in CMakeLists.txt and Doxyfile -3. Go into `umrt-build` package settings and give the new repo read permission -4. Go into `umrt-apt-image` package settings and give the new repo read permission -5. Go into `UMRoboticsTeam` organisation secrets and add the new repo to: - - `APT_DEPLOY_KEY` - - `APT_SIGNING_KEY` -6. Copy the rulesets (branch protection rules) from a mature repository like - [umrt-arm-firmware-lib](https://github.com/UMRoboticsTeam/umrt-arm-firmware-lib/) -7. Remove this notice and fill in below README template -8. Write something in mainpage.dox -9. Replace example files with real code, add source files to CMake targets, document it with Doxygen, and proceed +Unlike most UMRT projects, this one deviates from the norm of one-package-per-repo. +This is because of how tightly coupled the packages here are. +Whenever a change is made to one config, it is likely to propagate into other configs, which becomes tedious to manage when they are all individually controlled and versioned. ---- -# Project Name - -This library/executable/project implements XYZ functionality for the University of Manitoba Robotics Team's -rover/robotic arm. - -[See the documentation](https://umroboticsteam.github.io/********** project-name **********/) \ No newline at end of file +For example, if a URDF is changed, that would affect the ros2_control configuration, the IKFast configuration, and the MoveIt configuration. +If each of these lived in separate repos, that is 4 PRs to create, 4 new versions which have to be deployed, and if one doesn't get updated somewhere it will cause major problems. +Thus, by organising these into a monorepo this is condensed to 1 PR, and 1 version to deploy/manage. \ No newline at end of file From 51d6e3b2448d04100487d4584d5defd0147d4a7f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 22:15:01 -0500 Subject: [PATCH 02/24] Removed ROS package structure since this will be a monorepo --- CMakeLists.txt | 61 --------------------------- include/project-name/example_node.hpp | 23 ---------- launch/example.launch.py | 21 --------- launch/example.yaml | 3 -- package.in.xml | 21 --------- src/example_node.cpp | 9 ---- src/project_name.cpp | 14 ------ 7 files changed, 152 deletions(-) delete mode 100644 CMakeLists.txt delete mode 100644 include/project-name/example_node.hpp delete mode 100644 launch/example.launch.py delete mode 100644 launch/example.yaml delete mode 100644 package.in.xml delete mode 100644 src/example_node.cpp delete mode 100644 src/project_name.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index a5e21a5..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(********** Fill in project name **********) - -set(CMAKE_CXX_STANDARD 14) - -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -fPIC) -endif() - -# Fixes intellisense not finding ros2 headers https://youtrack.jetbrains.com/issue/CPP-29747/Certain-ROS2-package-headers-missing-from-Intellisense-when-using-a-Docker-toolchain -include_directories(SYSTEM /opt/ros/$ENV{ROS_DISTRO}/include) - -# find dependencies -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclcpp_lifecycle - ) - -# find dependencies -find_package(ament_cmake REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_executable(${PROJECT_NAME} - src/project_name.cpp - src/example_node.cpp -) -target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - ) -ament_target_dependencies( - ${PROJECT_NAME} PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) - -# Parse version number from the file -file(READ "version" version_input) -string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) - -# Fill version number into package.xml -configure_file( - package.in.xml - ${CMAKE_CURRENT_SOURCE_DIR}/package.xml - @ONLY -) - -# INSTALL -install(TARGETS - ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} - ) - -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/include/project-name/example_node.hpp b/include/project-name/example_node.hpp deleted file mode 100644 index 98efb60..0000000 --- a/include/project-name/example_node.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef PROJECT_NAME_EXAMPLE_NODE_HPP -#define PROJECT_NAME_EXAMPLE_NODE_HPP - -#include - -/** - * This is an example node provided to demonstrate file structure and CMake configuration. - */ -class ExampleNode : public rclcpp::Node { -public: - /** - * Constructs an ExampleNode. - */ - ExampleNode(); - - /** - * Prints a string to stdout. - */ - void example(); -}; - - -#endif //PROJECT_NAME_EXAMPLE_NODE_HPP diff --git a/launch/example.launch.py b/launch/example.launch.py deleted file mode 100644 index 19de1d8..0000000 --- a/launch/example.launch.py +++ /dev/null @@ -1,21 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -import os -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - - config = os.path.join(get_package_share_directory('project-name'),'launch', 'example.yaml') - - example_node = Node( - package='project-name', - executable='project-name', - name = 'umrt_example_node', - parameters=[config] - ) - - return LaunchDescription([ - example_node - ]) - diff --git a/launch/example.yaml b/launch/example.yaml deleted file mode 100644 index 116ea5c..0000000 --- a/launch/example.yaml +++ /dev/null @@ -1,3 +0,0 @@ -umrt_example_node: - ros__parameters: - example_param: 0 diff --git a/package.in.xml b/package.in.xml deleted file mode 100644 index 78a0a34..0000000 --- a/package.in.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ********** Fill in project name ********** - @version_input@ - ********** Fill in project description ********** - ***** First Last ***** - MPL-2.0 - - ament_cmake - - rclcpp - rclcpp_lifecycle - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/example_node.cpp b/src/example_node.cpp deleted file mode 100644 index 0e2066b..0000000 --- a/src/example_node.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "project-name/example_node.hpp" - -#include - -ExampleNode::ExampleNode() : Node("example") {} - -void ExampleNode::example() { - std::cout << "Hello World" << std::endl; -} diff --git a/src/project_name.cpp b/src/project_name.cpp deleted file mode 100644 index 533c57c..0000000 --- a/src/project_name.cpp +++ /dev/null @@ -1,14 +0,0 @@ -// -// Created by Noah on 2024-08-18. -// - -#include - -#include "project-name/node.hpp" - -int main(int argc, char* argv[]){ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file From a4d980638f607fc38b7df3025b909f3dfff5a8c6 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 22:58:37 -0500 Subject: [PATCH 03/24] Added Project Perry description files from umrt-arm-ros-firmware experimentation, which actually models what Prairie Pioneer looked like --- umrt-project-perry-description/CMakeLists.txt | 11 + .../rviz/project_perry.rviz | 196 +++++++++++++ .../rviz/project_perry_view.rviz | 192 +++++++++++++ .../urdf/project_perry.materials.xacro | 44 +++ .../urdf/project_perry_description.urdf.xacro | 272 ++++++++++++++++++ 5 files changed, 715 insertions(+) create mode 100644 umrt-project-perry-description/CMakeLists.txt create mode 100644 umrt-project-perry-description/rviz/project_perry.rviz create mode 100644 umrt-project-perry-description/rviz/project_perry_view.rviz create mode 100644 umrt-project-perry-description/urdf/project_perry.materials.xacro create mode 100644 umrt-project-perry-description/urdf/project_perry_description.urdf.xacro diff --git a/umrt-project-perry-description/CMakeLists.txt b/umrt-project-perry-description/CMakeLists.txt new file mode 100644 index 0000000..8b9d6cc --- /dev/null +++ b/umrt-project-perry-description/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_control_demo_description) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY urdf rviz + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/umrt-project-perry-description/rviz/project_perry.rviz b/umrt-project-perry-description/rviz/project_perry.rviz new file mode 100644 index 0000000..f96ea2e --- /dev/null +++ b/umrt-project-perry-description/rviz/project_perry.rviz @@ -0,0 +1,196 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1096 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + humerus_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + finger_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + finger_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.443930625915527 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0044944556429982185 + Y: 1.0785865783691406 + Z: 2.4839563369750977 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23039916157722473 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.150422096252441 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1379 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000004f00000010101000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000048000004f0000000db01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1470 \ No newline at end of file diff --git a/umrt-project-perry-description/rviz/project_perry_view.rviz b/umrt-project-perry-description/rviz/project_perry_view.rviz new file mode 100644 index 0000000..57803b5 --- /dev/null +++ b/umrt-project-perry-description/rviz/project_perry_view.rviz @@ -0,0 +1,192 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1096 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + humerus_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.443930625915527 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0044944556429982185 + Y: 1.0785865783691406 + Z: 2.4839563369750977 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23039916157722473 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.150422096252441 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1379 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000004f00000010101000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000048000004f0000000db01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1470 \ No newline at end of file diff --git a/umrt-project-perry-description/urdf/project_perry.materials.xacro b/umrt-project-perry-description/urdf/project_perry.materials.xacro new file mode 100644 index 0000000..b7f1f7c --- /dev/null +++ b/umrt-project-perry-description/urdf/project_perry.materials.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/umrt-project-perry-description/urdf/project_perry_description.urdf.xacro b/umrt-project-perry-description/urdf/project_perry_description.urdf.xacro new file mode 100644 index 0000000..2227d79 --- /dev/null +++ b/umrt-project-perry-description/urdf/project_perry_description.urdf.xacro @@ -0,0 +1,272 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 715a037b3220ac18f3a0ef664b9ca3007b00590b Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:15:53 -0500 Subject: [PATCH 04/24] Converted umrt-project-perry-description to standard CMake setup --- umrt-project-perry-description/.gitignore | 5 +++ umrt-project-perry-description/CMakeLists.txt | 34 +++++++++++++++++-- umrt-project-perry-description/package.in.xml | 17 ++++++++++ 3 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 umrt-project-perry-description/.gitignore create mode 100644 umrt-project-perry-description/package.in.xml diff --git a/umrt-project-perry-description/.gitignore b/umrt-project-perry-description/.gitignore new file mode 100644 index 0000000..ac37e03 --- /dev/null +++ b/umrt-project-perry-description/.gitignore @@ -0,0 +1,5 @@ +/cmake-build-debug-umrt-arm-ros-dev/ +/.idea/ +/output-doxygen/ +# We ignore package.xml because it is generated from package.in.xml +package.xml \ No newline at end of file diff --git a/umrt-project-perry-description/CMakeLists.txt b/umrt-project-perry-description/CMakeLists.txt index 8b9d6cc..f42f867 100644 --- a/umrt-project-perry-description/CMakeLists.txt +++ b/umrt-project-perry-description/CMakeLists.txt @@ -1,8 +1,38 @@ -cmake_minimum_required(VERSION 3.5) -project(ros2_control_demo_description) +cmake_minimum_required(VERSION 3.16) +project(umrt-project-perry-description) +set(CMAKE_CXX_STANDARD 14) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -fPIC) +endif() + +# Fixes intellisense not finding ros2 headers https://youtrack.jetbrains.com/issue/CPP-29747/Certain-ROS2-package-headers-missing-from-Intellisense-when-using-a-Docker-toolchain +include_directories(SYSTEM /opt/ros/$ENV{ROS_DISTRO}/include) + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS +) + +# find dependencies find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# Parse version number from the file +file(READ "../version" version_input) +string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) + +# Fill version number into package.xml +configure_file( + package.in.xml + ${CMAKE_CURRENT_SOURCE_DIR}/package.xml + @ONLY +) + +# INSTALL install( DIRECTORY urdf rviz DESTINATION share/${PROJECT_NAME}/ diff --git a/umrt-project-perry-description/package.in.xml b/umrt-project-perry-description/package.in.xml new file mode 100644 index 0000000..cdf6263 --- /dev/null +++ b/umrt-project-perry-description/package.in.xml @@ -0,0 +1,17 @@ + + + + umrt-project-perry-description + @version_input@ + Package with URDF and description files for Project Perry of the University of Manitoba Robotics Team. + + Noah Reeder + + MPLv2 + + ament_cmake + + + ament_cmake + + From 1fbae21deaeb9146bd8eb44d7694142c874b2002 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:19:50 -0500 Subject: [PATCH 05/24] Added Project Perry MoveIt config package generated from the MoveIt Setup Assistant. This includes some modifications made during my sim experimentation --- .../.setup_assistant | 25 +++++++++ .../CMakeLists.txt | 16 ++++++ .../config/initial_positions.yaml | 7 +++ .../config/joint_limits.yaml | 35 +++++++++++++ .../config/kinematics.yaml | 5 ++ .../config/moveit.rviz | 51 +++++++++++++++++++ .../config/moveit_controllers.yaml | 23 +++++++++ .../config/pilz_cartesian_limits.yaml | 6 +++ .../config/project_perry.srdf | 48 +++++++++++++++++ .../config/ros2_controllers.yaml | 31 +++++++++++ .../config/sensors_3d.yaml | 23 +++++++++ .../launch/demo.launch.py | 7 +++ .../launch/move_group.launch.py | 7 +++ .../launch/moveit_rviz.launch.py | 7 +++ .../launch/rsp.launch.py | 7 +++ .../launch/setup_assistant.launch.py | 7 +++ .../launch/spawn_controllers.launch.py | 7 +++ .../launch/static_virtual_joint_tfs.launch.py | 7 +++ .../launch/warehouse_db.launch.py | 7 +++ umrt-project-perry-moveit-config/pp.srdf.old | 50 ++++++++++++++++++ 20 files changed, 376 insertions(+) create mode 100644 umrt-project-perry-moveit-config/.setup_assistant create mode 100644 umrt-project-perry-moveit-config/CMakeLists.txt create mode 100644 umrt-project-perry-moveit-config/config/initial_positions.yaml create mode 100644 umrt-project-perry-moveit-config/config/joint_limits.yaml create mode 100644 umrt-project-perry-moveit-config/config/kinematics.yaml create mode 100644 umrt-project-perry-moveit-config/config/moveit.rviz create mode 100644 umrt-project-perry-moveit-config/config/moveit_controllers.yaml create mode 100644 umrt-project-perry-moveit-config/config/pilz_cartesian_limits.yaml create mode 100644 umrt-project-perry-moveit-config/config/project_perry.srdf create mode 100644 umrt-project-perry-moveit-config/config/ros2_controllers.yaml create mode 100644 umrt-project-perry-moveit-config/config/sensors_3d.yaml create mode 100644 umrt-project-perry-moveit-config/launch/demo.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/move_group.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/rsp.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/setup_assistant.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py create mode 100644 umrt-project-perry-moveit-config/launch/warehouse_db.launch.py create mode 100644 umrt-project-perry-moveit-config/pp.srdf.old diff --git a/umrt-project-perry-moveit-config/.setup_assistant b/umrt-project-perry-moveit-config/.setup_assistant new file mode 100644 index 0000000..1937839 --- /dev/null +++ b/umrt-project-perry-moveit-config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: umrt-arm-ros-firmware + relative_path: urdf/project_perry.urdf.xacro + srdf: + relative_path: config/project_perry.srdf + package_settings: + author_name: Noah Reeder + author_email: noahreederatc@gmail.com + generated_timestamp: 1750822039 + control_xacro: + command: + - velocity + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - velocity + state: + - position + - velocity \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/CMakeLists.txt b/umrt-project-perry-moveit-config/CMakeLists.txt new file mode 100644 index 0000000..b7fedcd --- /dev/null +++ b/umrt-project-perry-moveit-config/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.22) +project(project_perry_moveit_config) + +find_package(ament_cmake REQUIRED) + +ament_package() + +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") + install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +endif() + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/umrt-project-perry-moveit-config/config/initial_positions.yaml b/umrt-project-perry-moveit-config/config/initial_positions.yaml new file mode 100644 index 0000000..bf6f0ec --- /dev/null +++ b/umrt-project-perry-moveit-config/config/initial_positions.yaml @@ -0,0 +1,7 @@ +# Default initial positions for project_perry's ros2_control fake system + +initial_positions: + base_pitch_joint: 0 + base_yaw_joint: 0 + elbow_0_joint: 0 + gripper: 0 \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/config/joint_limits.yaml b/umrt-project-perry-moveit-config/config/joint_limits.yaml new file mode 100644 index 0000000..8e2b9ff --- /dev/null +++ b/umrt-project-perry-moveit-config/config/joint_limits.yaml @@ -0,0 +1,35 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + base_pitch_joint: + has_velocity_limits: true + max_velocity: 100.0 + has_acceleration_limits: false + max_acceleration: 0.0 + base_yaw_joint: + has_velocity_limits: true + max_velocity: 100.0 + has_acceleration_limits: false + max_acceleration: 0.0 + elbow_0_joint: + has_velocity_limits: true + max_velocity: 100.0 + has_acceleration_limits: false + max_acceleration: 0.0 + gripper: + has_velocity_limits: true + max_velocity: 100.0 + has_acceleration_limits: false + max_acceleration: 0.0 + gripper_mimic: + has_velocity_limits: true + max_velocity: 100.0 + has_acceleration_limits: false + max_acceleration: 0.0 \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/config/kinematics.yaml b/umrt-project-perry-moveit-config/config/kinematics.yaml new file mode 100644 index 0000000..507fb8a --- /dev/null +++ b/umrt-project-perry-moveit-config/config/kinematics.yaml @@ -0,0 +1,5 @@ +project_perry: + kinematics_solver: project_perry_project_perry/IKFastKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.5 + kinematic_solver_attempts: 3 diff --git a/umrt-project-perry-moveit-config/config/moveit.rviz b/umrt-project-perry-moveit-config/config/moveit.rviz new file mode 100644 index 0000000..f31651e --- /dev/null +++ b/umrt-project-perry-moveit-config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/umrt-project-perry-moveit-config/config/moveit_controllers.yaml b/umrt-project-perry-moveit-config/config/moveit_controllers.yaml new file mode 100644 index 0000000..06d4ded --- /dev/null +++ b/umrt-project-perry-moveit-config/config/moveit_controllers.yaml @@ -0,0 +1,23 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - arm_base_controller + - gripper_controller + + arm_base_controller: + type: FollowJointTrajectory + joints: + - base_yaw_joint + - base_pitch_joint + - elbow_0_joint + action_ns: follow_joint_trajectory + default: true + gripper_controller: + type: GripperCommand + joints: + - gripper + action_ns: gripper_cmd + default: true \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/config/pilz_cartesian_limits.yaml b/umrt-project-perry-moveit-config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/umrt-project-perry-moveit-config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/umrt-project-perry-moveit-config/config/project_perry.srdf b/umrt-project-perry-moveit-config/config/project_perry.srdf new file mode 100644 index 0000000..62fb817 --- /dev/null +++ b/umrt-project-perry-moveit-config/config/project_perry.srdf @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/umrt-project-perry-moveit-config/config/ros2_controllers.yaml b/umrt-project-perry-moveit-config/config/ros2_controllers.yaml new file mode 100644 index 0000000..56fe00d --- /dev/null +++ b/umrt-project-perry-moveit-config/config/ros2_controllers.yaml @@ -0,0 +1,31 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + arm_base_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + gripper_controller: + type: position_controllers/GripperActionController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +arm_base_controller: + ros__parameters: + joints: + - base_yaw_joint + - base_pitch_joint + - elbow_0_joint + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity +gripper_controller: + ros__parameters: + joint: gripper \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/config/sensors_3d.yaml b/umrt-project-perry-moveit-config/config/sensors_3d.yaml new file mode 100644 index 0000000..5d90e51 --- /dev/null +++ b/umrt-project-perry-moveit-config/config/sensors_3d.yaml @@ -0,0 +1,23 @@ +sensors: + - kinect_pointcloud + - kinect_depthimage +kinect_pointcloud: + filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater +kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/launch/demo.launch.py b/umrt-project-perry-moveit-config/launch/demo.launch.py new file mode 100644 index 0000000..8946405 --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/move_group.launch.py b/umrt-project-perry-moveit-config/launch/move_group.launch.py new file mode 100644 index 0000000..299d2e0 --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py b/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..b712a41 --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/rsp.launch.py b/umrt-project-perry-moveit-config/launch/rsp.launch.py new file mode 100644 index 0000000..15b6f44 --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py b/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..12eb91d --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py b/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..85ec761 --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py b/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..e68d2e7 --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py b/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py new file mode 100644 index 0000000..cf6a99b --- /dev/null +++ b/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/pp.srdf.old b/umrt-project-perry-moveit-config/pp.srdf.old new file mode 100644 index 0000000..d9ba045 --- /dev/null +++ b/umrt-project-perry-moveit-config/pp.srdf.old @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 5ee19df2c4e5a860e282340cd42a6a8f84dbd8d9 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:24:20 -0500 Subject: [PATCH 06/24] Renamed project to umrt-project-perry-moveit-config --- umrt-project-perry-moveit-config/.gitignore | 5 +++++ umrt-project-perry-moveit-config/CMakeLists.txt | 2 +- umrt-project-perry-moveit-config/launch/demo.launch.py | 2 +- umrt-project-perry-moveit-config/launch/move_group.launch.py | 2 +- .../launch/moveit_rviz.launch.py | 2 +- umrt-project-perry-moveit-config/launch/rsp.launch.py | 2 +- .../launch/setup_assistant.launch.py | 2 +- .../launch/spawn_controllers.launch.py | 2 +- .../launch/static_virtual_joint_tfs.launch.py | 2 +- .../launch/warehouse_db.launch.py | 2 +- 10 files changed, 14 insertions(+), 9 deletions(-) create mode 100644 umrt-project-perry-moveit-config/.gitignore diff --git a/umrt-project-perry-moveit-config/.gitignore b/umrt-project-perry-moveit-config/.gitignore new file mode 100644 index 0000000..224cb50 --- /dev/null +++ b/umrt-project-perry-moveit-config/.gitignore @@ -0,0 +1,5 @@ +/cmake-build-*/ +/.idea/ +/output-doxygen/ +# We ignore package.xml because it is generated from package.in.xml +package.xml \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/CMakeLists.txt b/umrt-project-perry-moveit-config/CMakeLists.txt index b7fedcd..c3841e7 100644 --- a/umrt-project-perry-moveit-config/CMakeLists.txt +++ b/umrt-project-perry-moveit-config/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.22) -project(project_perry_moveit_config) +project(umrt-project-perry-moveit-config) find_package(ament_cmake REQUIRED) diff --git a/umrt-project-perry-moveit-config/launch/demo.launch.py b/umrt-project-perry-moveit-config/launch/demo.launch.py index 8946405..79c60ee 100644 --- a/umrt-project-perry-moveit-config/launch/demo.launch.py +++ b/umrt-project-perry-moveit-config/launch/demo.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_demo_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/move_group.launch.py b/umrt-project-perry-moveit-config/launch/move_group.launch.py index 299d2e0..cbc1707 100644 --- a/umrt-project-perry-moveit-config/launch/move_group.launch.py +++ b/umrt-project-perry-moveit-config/launch/move_group.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_move_group_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py b/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py index b712a41..6f6769f 100644 --- a/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py +++ b/umrt-project-perry-moveit-config/launch/moveit_rviz.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/rsp.launch.py b/umrt-project-perry-moveit-config/launch/rsp.launch.py index 15b6f44..4e8f9f7 100644 --- a/umrt-project-perry-moveit-config/launch/rsp.launch.py +++ b/umrt-project-perry-moveit-config/launch/rsp.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_rsp_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py b/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py index 12eb91d..02658c0 100644 --- a/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py +++ b/umrt-project-perry-moveit-config/launch/setup_assistant.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py b/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py index 85ec761..270fd83 100644 --- a/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py +++ b/umrt-project-perry-moveit-config/launch/spawn_controllers.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_spawn_controllers_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py b/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py index e68d2e7..54c991e 100644 --- a/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py +++ b/umrt-project-perry-moveit-config/launch/static_virtual_joint_tfs.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py b/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py index cf6a99b..5b166dc 100644 --- a/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py +++ b/umrt-project-perry-moveit-config/launch/warehouse_db.launch.py @@ -3,5 +3,5 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("project_perry", package_name="project_perry_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("project_perry", package_name="umrt-project-perry-moveit-config").to_moveit_configs() return generate_warehouse_db_launch(moveit_config) From cfbda5bf11971a1c6bd799274740dc75e9b0c088 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:26:26 -0500 Subject: [PATCH 07/24] Set up CMake for umrt-project-perry-moveit-config to parse version number like other projects --- .../CMakeLists.txt | 11 ++++ .../package.in.xml | 52 +++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 umrt-project-perry-moveit-config/package.in.xml diff --git a/umrt-project-perry-moveit-config/CMakeLists.txt b/umrt-project-perry-moveit-config/CMakeLists.txt index c3841e7..722f6c6 100644 --- a/umrt-project-perry-moveit-config/CMakeLists.txt +++ b/umrt-project-perry-moveit-config/CMakeLists.txt @@ -3,6 +3,17 @@ project(umrt-project-perry-moveit-config) find_package(ament_cmake REQUIRED) +# Parse version number from the file +file(READ "../version" version_input) +string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) + +# Fill version number into package.xml +configure_file( + package.in.xml + ${CMAKE_CURRENT_SOURCE_DIR}/package.xml + @ONLY +) + ament_package() if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") diff --git a/umrt-project-perry-moveit-config/package.in.xml b/umrt-project-perry-moveit-config/package.in.xml new file mode 100644 index 0000000..9d3dbc5 --- /dev/null +++ b/umrt-project-perry-moveit-config/package.in.xml @@ -0,0 +1,52 @@ + + + + umrt-project-perry-moveit-config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the project_perry with the MoveIt Motion Planning Framework + + Noah Reeder + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + Noah Reeder + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + umrt-arm-ros-firmware + warehouse_ros_mongo + xacro + + + + ament_cmake + + From 71991a3a9a3dc6f7df8283b262e09040cb05887c Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:40:58 -0500 Subject: [PATCH 08/24] Added Project Perry IKFast plugin from my sim experimentation --- README.md | 17 +- .../.gitignore | 5 + .../CMakeLists.txt | 48 + .../include/ikfast.h | 378 ++ ...perry_moveit_ikfast_plugin_description.xml | 6 + ...rry_project_perry_ikfast_moveit_plugin.cpp | 1406 +++++ ...ject_perry_project_perry_ikfast_solver.cpp | 5489 +++++++++++++++++ .../update_ikfast_plugin.sh | 22 + 8 files changed, 7370 insertions(+), 1 deletion(-) create mode 100644 project_perry_project_perry_ikfast_plugin/.gitignore create mode 100644 project_perry_project_perry_ikfast_plugin/CMakeLists.txt create mode 100644 project_perry_project_perry_ikfast_plugin/include/ikfast.h create mode 100644 project_perry_project_perry_ikfast_plugin/project_perry_project_perry_moveit_ikfast_plugin_description.xml create mode 100644 project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_moveit_plugin.cpp create mode 100644 project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_solver.cpp create mode 100644 project_perry_project_perry_ikfast_plugin/update_ikfast_plugin.sh diff --git a/README.md b/README.md index 1e0790f..7aa0411 100644 --- a/README.md +++ b/README.md @@ -7,4 +7,19 @@ Whenever a change is made to one config, it is likely to propagate into other co For example, if a URDF is changed, that would affect the ros2_control configuration, the IKFast configuration, and the MoveIt configuration. If each of these lived in separate repos, that is 4 PRs to create, 4 new versions which have to be deployed, and if one doesn't get updated somewhere it will cause major problems. -Thus, by organising these into a monorepo this is condensed to 1 PR, and 1 version to deploy/manage. \ No newline at end of file +Thus, by organising these into a monorepo this is condensed to 1 PR, and 1 version to deploy/manage. + +# UMRT Project Perry Description + +This is the URDF and RViz files for the Project Perry robotics arm model. + +# UMRT Project Perry MoveIt Config + +This is the MoveIt2 config for interacting with the Project Perry robotic arm. +It was generated using the MoveIt Setup Assistant. + +# UMRT Project Perry IKFast Plugin + +This is the IKFast plugin MoveIt2 uses for computing inverse kinematic solutions for the Project Perry robotic arm. +Unlike other UMRT packages, the package name is separated with underscores instead of hyphens because the package is auto-generated and the generator uses underscores. +The package is generated using the moveit_kinematics auto_create_ikfast_moveit_plugin script according to the instructions [here](https://moveit.picknik.ai/main/doc/examples/ikfast/ikfast_tutorial.html). \ No newline at end of file diff --git a/project_perry_project_perry_ikfast_plugin/.gitignore b/project_perry_project_perry_ikfast_plugin/.gitignore new file mode 100644 index 0000000..224cb50 --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/.gitignore @@ -0,0 +1,5 @@ +/cmake-build-*/ +/.idea/ +/output-doxygen/ +# We ignore package.xml because it is generated from package.in.xml +package.xml \ No newline at end of file diff --git a/project_perry_project_perry_ikfast_plugin/CMakeLists.txt b/project_perry_project_perry_ikfast_plugin/CMakeLists.txt new file mode 100644 index 0000000..d040cbb --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.22) +project(project_perry_project_perry_ikfast_plugin) + +if(NOT "${CMAKE_CXX_STANDARD}") + set(CMAKE_CXX_STANDARD 14) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(ament_cmake REQUIRED) +find_package(moveit_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_kdl REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(LAPACK REQUIRED) + +include_directories(include) + +set(IKFAST_LIBRARY_NAME project_perry_project_perry_moveit_ikfast_plugin) +add_library(${IKFAST_LIBRARY_NAME} SHARED src/project_perry_project_perry_ikfast_moveit_plugin.cpp) +ament_target_dependencies(${IKFAST_LIBRARY_NAME} + rclcpp + moveit_core + pluginlib + tf2_kdl + orocos_kdl + tf2_eigen + LAPACK +) +# suppress warnings about unused variables in OpenRave's solver code +target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable -Wno-unused-parameter) + +install(TARGETS ${IKFAST_LIBRARY_NAME} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +pluginlib_export_plugin_description_file(moveit_core project_perry_project_perry_moveit_ikfast_plugin_description.xml) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(moveit_core) +ament_export_dependencies(pluginlib) +ament_export_dependencies(rclcpp) +ament_export_dependencies(tf2_kdl) +ament_export_dependencies(tf2_eigen) +ament_package() diff --git a/project_perry_project_perry_ikfast_plugin/include/ikfast.h b/project_perry_project_perry_ikfast_plugin/include/ikfast.h new file mode 100644 index 0000000..82f9429 --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/include/ikfast.h @@ -0,0 +1,378 @@ +// -*- coding: utf-8 -*- +// Copyright (C) 2012 Rosen Diankov +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/** \brief Header file for all ikfast c++ files/shared objects. + + The ikfast inverse kinematics compiler is part of OpenRAVE. + + The file is divided into two sections: + - Common - the abstract classes section that all ikfast share regardless of their settings + - Library Specific - the library-specific definitions, which depends on the precision/settings that the + library was compiled with + + The defines are as follows, they are also used for the ikfast C++ class: + + - IKFAST_HEADER_COMMON - common classes + - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off + - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility. + - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY + - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid + conditions are detected. + - IKFAST_REAL - Use to force a custom real number type for IkReal. + - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded. + + */ + +#pragma once + +#include +#include +#include + +#pragma once + +/// should be the same as ikfast.__version__ +#define IKFAST_VERSION 61 + +namespace ikfast +{ +/// \brief holds the solution for a single dof +template +class IkSingleDOFSolutionBase +{ +public: + IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) + { + indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; + } + T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset + signed char freeind; ///< if >= 0, mimics another joint + unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider + unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself + unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a + /// solution can be repeated for different indices. store at least another repeated root +}; + +/// \brief The discrete solutions are returned in this structure. +/// +/// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. +/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its +/// prototype is: +template +class IkSolutionBase +{ +public: + virtual ~IkSolutionBase() + { + } + /// \brief gets a concrete solution + /// + /// \param[out] solution the result + /// \param[in] freevalues values for the free parameters \se GetFree + virtual void GetSolution(T* solution, const T* freevalues) const = 0; + + /// \brief std::vector version of \ref GetSolution + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : nullptr); + } + + /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned + /// + /// \return vector of indices indicating the free parameters + virtual const std::vector& GetFree() const = 0; + + /// \brief the dof of the solution + virtual int GetDOF() const = 0; +}; + +/// \brief manages all the solutions +template +class IkSolutionListBase +{ +public: + virtual ~IkSolutionListBase() + { + } + + /// \brief add one solution and return its index for later retrieval + /// + /// \param vinfos Solution data for each degree of freedom of the manipulator + /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can + /// freely set. + virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; + + /// \brief returns the solution pointer + virtual const IkSolutionBase& GetSolution(size_t index) const = 0; + + /// \brief returns the number of solutions stored + virtual size_t GetNumSolutions() const = 0; + + /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be + /// invalidated. + virtual void Clear() = 0; +}; + +/// \brief holds function pointers for all the exported functions of ikfast +template +class IkFastFunctions +{ +public: + IkFastFunctions() + : _ComputeIk(nullptr) + , _ComputeFk(nullptr) + , _GetNumFreeParameters(nullptr) + , _GetFreeParameters(nullptr) + , _GetNumJoints(nullptr) + , _GetIkRealSize(nullptr) + , _GetIkFastVersion(nullptr) + , _GetIkType(nullptr) + , _GetKinematicsHash(nullptr) + { + } + virtual ~IkFastFunctions() + { + } + typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); + ComputeIkFn _ComputeIk; + typedef void (*ComputeFkFn)(const T*, T*, T*); + ComputeFkFn _ComputeFk; + typedef int (*GetNumFreeParametersFn)(); + GetNumFreeParametersFn _GetNumFreeParameters; + typedef int* (*GetFreeParametersFn)(); + GetFreeParametersFn _GetFreeParameters; + typedef int (*GetNumJointsFn)(); + GetNumJointsFn _GetNumJoints; + typedef int (*GetIkRealSizeFn)(); + GetIkRealSizeFn _GetIkRealSize; + typedef const char* (*GetIkFastVersionFn)(); + GetIkFastVersionFn _GetIkFastVersion; + typedef int (*GetIkTypeFn)(); + GetIkTypeFn _GetIkType; + typedef const char* (*GetKinematicsHashFn)(); + GetKinematicsHashFn _GetKinematicsHash; +}; + +// Implementations of the abstract classes, user doesn't need to use them + +/// \brief Default implementation of \ref IkSolutionBase +template +class IkSolution : public IkSolutionBase +{ +public: + IkSolution(const std::vector >& vinfos, const std::vector& vfree) + { + _vbasesol = vinfos; + _vfree = vfree; + } + + virtual void GetSolution(T* solution, const T* freevalues) const + { + for (std::size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].freeind < 0) + solution[i] = _vbasesol[i].foffset; + else + { + solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset; + if (solution[i] > T(3.14159265358979)) + { + solution[i] -= T(6.28318530717959); + } + else if (solution[i] < T(-3.14159265358979)) + { + solution[i] += T(6.28318530717959); + } + } + } + } + + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : nullptr); + } + + virtual const std::vector& GetFree() const + { + return _vfree; + } + virtual int GetDOF() const + { + return static_cast(_vbasesol.size()); + } + + virtual void Validate() const + { + for (size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].maxsolutions == (unsigned char)-1) + { + throw std::runtime_error("max solutions for joint not initialized"); + } + if (_vbasesol[i].maxsolutions > 0) + { + if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("index >= max solutions for joint"); + } + if (_vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("2nd index >= max solutions for joint"); + } + } + } + } + + virtual void GetSolutionIndices(std::vector& v) const + { + v.resize(0); + v.push_back(0); + for (int i = static_cast(_vbasesol.size()) - 1; i >= 0; --i) + { + if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1) + { + for (size_t j = 0; j < v.size(); ++j) + { + v[j] *= _vbasesol[i].maxsolutions; + } + size_t orgsize = v.size(); + if (_vbasesol[i].indices[1] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v.push_back(v[j] + _vbasesol[i].indices[1]); + } + } + if (_vbasesol[i].indices[0] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v[j] += _vbasesol[i].indices[0]; + } + } + } + } + } + + std::vector > _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector _vfree; +}; + +/// \brief Default implementation of \ref IkSolutionListBase +template +class IkSolutionList : public IkSolutionListBase +{ +public: + virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) + { + size_t index = _listsolutions.size(); + _listsolutions.push_back(IkSolution(vinfos, vfree)); + return index; + } + + virtual const IkSolutionBase& GetSolution(size_t index) const + { + if (index >= _listsolutions.size()) + { + throw std::runtime_error("GetSolution index is invalid"); + } + typename std::list >::const_iterator it = _listsolutions.begin(); + std::advance(it, index); + return *it; + } + + virtual size_t GetNumSolutions() const + { + return _listsolutions.size(); + } + + virtual void Clear() + { + _listsolutions.clear(); + } + +protected: + std::list > _listsolutions; +}; +} // namespace ikfast + +// The following code is dependent on the C++ library linking with. +#ifdef IKFAST_HAS_LIBRARY + +// defined when creating a shared object/dll +#ifdef IKFAST_CLIBRARY +#ifdef _MSC_VER +#define IKFAST_API extern "C" __declspec(dllexport) +#else +#define IKFAST_API extern "C" +#endif +#else +#define IKFAST_API +#endif + +#ifdef IKFAST_NAMESPACE +namespace IKFAST_NAMESPACE +{ +#endif + +#ifdef IKFAST_REAL +typedef IKFAST_REAL IkReal; +#else +typedef double IkReal; +#endif + +/** \brief Computes all IK solutions given a end effector coordinates and the free joints. + + - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. + - ``eerot`` + - For **Transform6D** it is 9 values for the 3x3 rotation matrix. + - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. + - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value + represents the angle. + - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end + effector coordinate system. + */ +IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, + ikfast::IkSolutionListBase& solutions); + +/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. +IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); + +/// \brief returns the number of free parameters users has to set apriori +IKFAST_API int GetNumFreeParameters(); + +/// \brief the indices of the free parameters indexed by the chain joints +IKFAST_API int* GetFreeParameters(); + +/// \brief the total number of indices of the chain +IKFAST_API int GetNumJoints(); + +/// \brief the size in bytes of the configured number type +IKFAST_API int GetIkRealSize(); + +/// \brief the ikfast version used to generate this file +IKFAST_API const char* GetIkFastVersion(); + +/// \brief the ik type ID +IKFAST_API int GetIkType(); + +/// \brief a hash of all the chain values used for double checking that the correct IK is used. +IKFAST_API const char* GetKinematicsHash(); + +#ifdef IKFAST_NAMESPACE +} +#endif + +#endif // IKFAST_HAS_LIBRARY diff --git a/project_perry_project_perry_ikfast_plugin/project_perry_project_perry_moveit_ikfast_plugin_description.xml b/project_perry_project_perry_ikfast_plugin/project_perry_project_perry_moveit_ikfast_plugin_description.xml new file mode 100644 index 0000000..536894e --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/project_perry_project_perry_moveit_ikfast_plugin_description.xml @@ -0,0 +1,6 @@ + + + + IKFast61 plugin for closed-form kinematics of project_perry project_perry + + diff --git a/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_moveit_plugin.cpp b/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_moveit_plugin.cpp new file mode 100644 index 0000000..2bc47df --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_moveit_plugin.cpp @@ -0,0 +1,1406 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer + *IPA + * 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 the all of the author's companies 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. + * + *********************************************************************/ + +/* + * IKFast Kinematics Solver Plugin for MoveIt wrapping an ikfast solver from OpenRAVE. + * + * AUTO-GENERATED by create_ikfast_moveit_plugin.py in moveit_kinematics package. + * + * This file, including the ikfast cpp from OpenRAVE below, forms a MoveIt kinematics plugin. + */ + +#include +#include +#include +#include +#include +#include + +using namespace moveit::core; + +// Need a floating point tolerance when checking joint limits, in case the joint starts at limit +const double LIMIT_TOLERANCE = .0000001; +/// \brief Search modes for searchPositionIK(), see there +enum SEARCH_MODE +{ + OPTIMIZE_FREE_JOINT = 1, + OPTIMIZE_MAX_JOINT = 2 +}; + +namespace project_perry_project_perry +{ +#define IKFAST_NO_MAIN // Don't include main() from IKFast + +/// \brief The types of inverse kinematics parameterizations supported. +/// +/// The minimum degree of freedoms required is set in the upper 4 bits of each type. +/// The number of values used to represent the parameterization ( >= dof ) is the next 4 bits. +/// The lower bits contain a unique id of the type. +enum IkParameterizationType +{ + IKP_None = 0, + IKP_Transform6D = 0x67000001, ///< end effector reaches desired 6D transformation + IKP_Rotation3D = 0x34000002, ///< end effector reaches desired 3D rotation + IKP_Translation3D = 0x33000003, ///< end effector origin reaches desired 3D translation + IKP_Direction3D = 0x23000004, ///< direction on end effector coordinate system reaches desired direction + IKP_Ray4D = 0x46000005, ///< ray on end effector coordinate system reaches desired global ray + IKP_Lookat3D = 0x23000006, ///< direction on end effector coordinate system points to desired 3D position + IKP_TranslationDirection5D = 0x56000007, ///< end effector origin and direction reaches desired 3D translation and + /// direction. Can be thought of as Ray IK where the origin of the ray must + /// coincide. + IKP_TranslationXY2D = 0x22000008, ///< 2D translation along XY plane + IKP_TranslationXYOrientation3D = 0x33000009, ///< 2D translation along XY plane and 1D rotation around Z axis. The + /// offset of the rotation is measured starting at +X, so at +X is it 0, + /// at +Y it is pi/2. + IKP_TranslationLocalGlobal6D = 0x3600000a, ///< local point on end effector origin reaches desired 3D global point + + IKP_TranslationXAxisAngle4D = 0x4400000b, ///< end effector origin reaches desired 3D translation, manipulator + /// direction makes a specific angle with x-axis like a cone, angle is from + /// 0-pi. Axes defined in the manipulator base link's coordinate system) + IKP_TranslationYAxisAngle4D = 0x4400000c, ///< end effector origin reaches desired 3D translation, manipulator + /// direction makes a specific angle with y-axis like a cone, angle is from + /// 0-pi. Axes defined in the manipulator base link's coordinate system) + IKP_TranslationZAxisAngle4D = 0x4400000d, ///< end effector origin reaches desired 3D translation, manipulator + /// direction makes a specific angle with z-axis like a cone, angle is from + /// 0-pi. Axes are defined in the manipulator base link's coordinate system. + + IKP_TranslationXAxisAngleZNorm4D = 0x4400000e, ///< end effector origin reaches desired 3D translation, manipulator + /// direction needs to be orthogonal to z-axis and be rotated at a + /// certain angle starting from the x-axis (defined in the manipulator + /// base link's coordinate system) + IKP_TranslationYAxisAngleXNorm4D = 0x4400000f, ///< end effector origin reaches desired 3D translation, manipulator + /// direction needs to be orthogonal to x-axis and be rotated at a + /// certain angle starting from the y-axis (defined in the manipulator + /// base link's coordinate system) + IKP_TranslationZAxisAngleYNorm4D = 0x44000010, ///< end effector origin reaches desired 3D translation, manipulator + /// direction needs to be orthogonal to y-axis and be rotated at a + /// certain angle starting from the z-axis (defined in the manipulator + /// base link's coordinate system) + + IKP_NumberOfParameterizations = 16, ///< number of parameterizations (does not count IKP_None) + + IKP_VelocityDataBit = + 0x00008000, ///< bit is set if the data represents the time-derivate velocity of an IkParameterization + IKP_Transform6DVelocity = IKP_Transform6D | IKP_VelocityDataBit, + IKP_Rotation3DVelocity = IKP_Rotation3D | IKP_VelocityDataBit, + IKP_Translation3DVelocity = IKP_Translation3D | IKP_VelocityDataBit, + IKP_Direction3DVelocity = IKP_Direction3D | IKP_VelocityDataBit, + IKP_Ray4DVelocity = IKP_Ray4D | IKP_VelocityDataBit, + IKP_Lookat3DVelocity = IKP_Lookat3D | IKP_VelocityDataBit, + IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D | IKP_VelocityDataBit, + IKP_TranslationXY2DVelocity = IKP_TranslationXY2D | IKP_VelocityDataBit, + IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D | IKP_VelocityDataBit, + IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D | IKP_VelocityDataBit, + IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D | IKP_VelocityDataBit, + IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D | IKP_VelocityDataBit, + IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D | IKP_VelocityDataBit, + IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D | IKP_VelocityDataBit, + IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D | IKP_VelocityDataBit, + IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D | IKP_VelocityDataBit, + + IKP_UniqueIdMask = 0x0000ffff, ///< the mask for the unique ids + IKP_CustomDataBit = 0x00010000, ///< bit is set if the ikparameterization contains custom data, this is only used + /// when serializing the ik parameterizations +}; + +// struct for storing and sorting solutions +struct LimitObeyingSol +{ + std::vector value; + double dist_from_seed; + + bool operator<(const LimitObeyingSol& a) const + { + return dist_from_seed < a.dist_from_seed; + } +}; + +// Code generated by IKFast56/61 +#include "project_perry_project_perry_ikfast_solver.cpp" + +class IKFastKinematicsPlugin : public kinematics::KinematicsBase +{ + std::vector joint_names_; + std::vector joint_min_vector_; + std::vector joint_max_vector_; + std::vector joint_has_limits_vector_; + std::vector link_names_; + const size_t num_joints_; + std::vector free_params_; + + // The ikfast and base frame are the start and end of the kinematic chain for which the + // IKFast analytic solution was generated. + const std::string IKFAST_TIP_FRAME_ = "gripper_link"; + const std::string IKFAST_BASE_FRAME_ = "base_link"; + + // prefix added to tip- and baseframe to allow different namespaces or multi-robot setups + std::string link_prefix_; + + // The transform tip and base bool are set to true if this solver is used with a kinematic + // chain that extends beyond the ikfast tip and base frame. The solution will be valid so + // long as there are no active, passive, or mimic joints between either the ikfast_tip_frame + // and the tip_frame of the group or the ikfast_base_frame and the base_frame for the group. + bool tip_transform_required_; + bool base_transform_required_; + + // We store the transform from the ikfast_base_frame to the group base_frame as well as the + // ikfast_tip_frame to the group tip_frame to transform input poses into the solver frame. + Eigen::Isometry3d chain_base_to_group_base_; + Eigen::Isometry3d group_tip_to_chain_tip_; + + bool initialized_; // Internal variable that indicates whether solvers are configured and ready + const rclcpp::Logger LOGGER = rclcpp::get_logger("project_perry_project_perry_ikfast_solver"); + + const std::vector& getJointNames() const override + { + return joint_names_; + } + const std::vector& getLinkNames() const override + { + return link_names_; + } + +public: + /** @class + * @brief Interface for an IKFast kinematics plugin + */ + IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), initialized_(false) + { + srand(time(nullptr)); + supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION); + supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED); + supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED); + } + + /** + * @brief Given a desired pose of the end-effector, compute the joint angles to reach it + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solution the solution vector + * @param error_code an error code that encodes the reason for failure or success + * @return True if a valid solution was found, false otherwise + */ + + // Returns the IK solution that is within joint limits closest to ik_seed_state + bool + getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, compute the set joint angles solutions that are able to reach it. + * + * This is a default implementation that returns only one solution and so its result is equivalent to calling + * 'getPositionIK(...)' with a zero initialized seed. + * + * @param ik_poses The desired pose of each tip link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solutions A vector of vectors where each entry is a valid joint solution + * @param result A struct that reports the results of the query + * @param options An option struct which contains the type of redundancy discretization used. This default + * implementation only supports the KinmaticSearches::NO_DISCRETIZATION method; requesting any + * other will result in failure. + * @return True if a valid set of solutions was found, false otherwise. + */ + bool getPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + std::vector>& solutions, kinematics::KinematicsResult& result, + const kinematics::KinematicsQueryOptions& options) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param the distance that the redundancy can be from the current position + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). The consistency_limit specifies that only certain redundancy positions + * around those specified in the seed state are admissible and need to be searched. + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param consistency_limit the distance that the redundancy can be from the current position + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a set of joint angles and a set of links, compute their pose + * + * @param link_names A set of links for which FK needs to be computed + * @param joint_angles The state for which FK is being computed + * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) + * @return True if a valid solution was found, false otherwise + */ + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const override; + + /** + * @brief Sets the discretization value for the redundant joint. + * + * Since this ikfast implementation allows for one redundant joint then only the first entry will be in the + *discretization map will be used. + * Calling this method replaces previous discretization settings. + * + * @param discretization a map of joint indices and discretization value pairs. + */ + void setSearchDiscretization(const std::map& discretization); + + /** + * @brief Overrides the default method to prevent changing the redundant joints + */ + bool setRedundantJoints(const std::vector& redundant_joint_indices) override; + +private: + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization) override; + + /** + * @brief Calls the IK solver from IKFast + * @return The number of solutions found + */ + size_t solve(KDL::Frame& pose_frame, const std::vector& vfree, IkSolutionList& solutions) const; + + /** + * @brief Gets a specific solution from the set + */ + void getSolution(const IkSolutionList& solutions, int i, std::vector& solution) const; + + /** + * @brief Gets a specific solution from the set with joints rotated 360° to be near seed state where possible + */ + void getSolution(const IkSolutionList& solutions, const std::vector& ik_seed_state, int i, + std::vector& solution) const; + + /** + * @brief If the value is outside of min/max then it tries to +/- 2 * pi to put the value into the range + */ + double enforceLimits(double val, double min, double max) const; + + void fillFreeParams(int count, int* array); + bool getCount(int& count, const int& max_count, const int& min_count) const; + + /** + * @brief samples the designated redundant joint using the chosen discretization method + * @param method An enumeration flag indicating the discretization method to be used + * @param sampled_joint_vals Sampled joint values for the redundant joint + * @return True if sampling succeeded. + */ + bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector& sampled_joint_vals) const; + + /// Validate that we can compute a fixed transform between from and to links. + bool computeRelativeTransform(const std::string& from, const std::string& to, Eigen::Isometry3d& transform, + bool& differs_from_identity); + /** + * @brief Transforms the input pose to the correct frame for the solver. This assumes that the group includes the + * entire solver chain and that any joints outside of the solver chain within the group are are fixed. + * @param ik_pose The pose to be transformed which should be in the correct frame for the group. + * @param ik_pose_chain The ik_pose to be populated with the appropriate pose for the solver + */ + void transformToChainFrame(const geometry_msgs::msg::Pose& ik_pose, KDL::Frame& ik_pose_chain) const; +}; // end class + +bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, const std::string& to, + Eigen::Isometry3d& transform, bool& differs_from_identity) +{ + RobotStatePtr robot_state; + robot_state = std::make_shared(robot_model_); + robot_state->setToDefaultValues(); + + bool has_link; // to suppress RCLCPP_ERRORs for non-existent frames + auto* from_link = robot_model_->getLinkModel(from, &has_link); + auto* to_link = robot_model_->getLinkModel(to, &has_link); + if (!from_link || !to_link) + return false; + + if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) != + robot_model_->getRigidlyConnectedParentLinkModel(to_link)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Link frames " << from << " and " << to << " are not rigidly connected."); + return false; + } + + transform = robot_state->getGlobalLinkTransform(from_link).inverse() * robot_state->getGlobalLinkTransform(to_link); + differs_from_identity = !transform.matrix().isIdentity(); + return true; +} + +bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, + const moveit::core::RobotModel& robot_model, const std::string& group_name, + const std::string& base_frame, const std::vector& tip_frames, + double search_discretization) +{ + if (tip_frames.size() != 1) + { + RCLCPP_ERROR(LOGGER, "Expecting exactly one tip frame."); + return false; + } + + storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); + if (!lookupParam(node, "link_prefix", link_prefix_, std::string(""))) + { + RCLCPP_INFO(LOGGER, "Using empty link_prefix."); + } + else + { + RCLCPP_INFO_STREAM(LOGGER, "Using link_prefix: '" << link_prefix_ << "'"); + } + + // verbose error output. subsequent checks in computeRelativeTransform return false then + if (!robot_model.hasLinkModel(tip_frames_[0])) + RCLCPP_ERROR_STREAM(LOGGER, "tip frame '" << tip_frames_[0] << "' does not exist."); + if (!robot_model.hasLinkModel(base_frame_)) + RCLCPP_ERROR_STREAM(LOGGER, "base_frame '" << base_frame_ << "' does not exist."); + + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_TIP_FRAME_)) + RCLCPP_ERROR_STREAM(LOGGER, "prefixed tip frame '" << link_prefix_ + IKFAST_TIP_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); + if (!robot_model.hasLinkModel(link_prefix_ + IKFAST_BASE_FRAME_)) + RCLCPP_ERROR_STREAM(LOGGER, "prefixed base frame '" << link_prefix_ + IKFAST_BASE_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); + // This IKFast solution was generated with IKFAST_TIP_FRAME_ and IKFAST_BASE_FRAME_. + // It is often the case that fixed joints are added to these links to model things like + // a robot mounted on a table or a robot with an end effector attached to the last link. + // To support these use cases, we store the transform from the IKFAST_BASE_FRAME_ to the + // base_frame_ and IKFAST_TIP_FRAME_ the tip_frame_ and transform to the input pose accordingly + if (!computeRelativeTransform(tip_frames_[0], link_prefix_ + IKFAST_TIP_FRAME_, group_tip_to_chain_tip_, + tip_transform_required_) || + !computeRelativeTransform(link_prefix_ + IKFAST_BASE_FRAME_, base_frame_, chain_base_to_group_base_, + base_transform_required_)) + { + return false; + } + + // IKFast56/61 + fillFreeParams(GetNumFreeParameters(), GetFreeParameters()); + + if (free_params_.size() > 1) + { + RCLCPP_ERROR(LOGGER, "Only one free joint parameter supported!"); + return false; + } + else if (free_params_.size() == 1) + { + redundant_joint_indices_.clear(); + redundant_joint_indices_.push_back(free_params_[0]); + KinematicsBase::setSearchDiscretization(search_discretization); + } + + const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); + if (!jmg) + { + RCLCPP_ERROR_STREAM(LOGGER, "Unknown planning group: " << group_name); + return false; + } + + RCLCPP_DEBUG(LOGGER, "Registering joints and links"); + const moveit::core::LinkModel* link = robot_model_->getLinkModel(tip_frames_[0]); + const moveit::core::LinkModel* base_link = robot_model_->getLinkModel(base_frame_); + while (link && link != base_link) + { + RCLCPP_DEBUG_STREAM(LOGGER, "Link " << link->getName()); + link_names_.push_back(link->getName()); + const moveit::core::JointModel* joint = link->getParentJointModel(); + if (joint->getType() != joint->UNKNOWN && joint->getType() != joint->FIXED && joint->getVariableCount() == 1) + { + RCLCPP_DEBUG_STREAM(LOGGER, "Adding joint " << joint->getName()); + + joint_names_.push_back(joint->getName()); + const moveit::core::VariableBounds& bounds = joint->getVariableBounds()[0]; + joint_has_limits_vector_.push_back(bounds.position_bounded_); + joint_min_vector_.push_back(bounds.min_position_); + joint_max_vector_.push_back(bounds.max_position_); + } + link = link->getParentLinkModel(); + } + + if (joint_names_.size() != num_joints_) + { + RCLCPP_FATAL(LOGGER, "Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match", joint_names_.size(), + num_joints_); + return false; + } + + std::reverse(link_names_.begin(), link_names_.end()); + std::reverse(joint_names_.begin(), joint_names_.end()); + std::reverse(joint_min_vector_.begin(), joint_min_vector_.end()); + std::reverse(joint_max_vector_.begin(), joint_max_vector_.end()); + std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end()); + + for (size_t joint_id = 0; joint_id < num_joints_; ++joint_id) + RCLCPP_DEBUG_STREAM(LOGGER, joint_names_[joint_id] << " " << joint_min_vector_[joint_id] << " " + << joint_max_vector_[joint_id] << " " + << joint_has_limits_vector_[joint_id]); + + initialized_ = true; + return true; +} + +void IKFastKinematicsPlugin::setSearchDiscretization(const std::map& discretization) +{ + if (discretization.empty()) + { + RCLCPP_ERROR(LOGGER, "The 'discretization' map is empty"); + return; + } + + if (redundant_joint_indices_.empty()) + { + RCLCPP_ERROR(LOGGER, "This group's solver doesn't support redundant joints"); + return; + } + + if (discretization.begin()->first != redundant_joint_indices_[0]) + { + std::string redundant_joint = joint_names_[free_params_[0]]; + RCLCPP_ERROR_STREAM(LOGGER, "Attempted to discretize a non-redundant joint " + << discretization.begin()->first << ", only joint '" << redundant_joint + << "' with index " << redundant_joint_indices_[0] << " is redundant."); + return; + } + + if (discretization.begin()->second <= 0.0) + { + RCLCPP_ERROR_STREAM(LOGGER, "Discretization can not takes values that are <= 0"); + return; + } + + redundant_joint_discretization_.clear(); + redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second; +} + +bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector& redundant_joint_indices) +{ + RCLCPP_ERROR(LOGGER, "Changing the redundant joints isn't permitted by this group's solver "); + return false; +} + +size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector& vfree, + IkSolutionList& solutions) const +{ + // IKFast56/61 + solutions.Clear(); + + double trans[3]; + trans[0] = pose_frame.p[0]; //-.18; + trans[1] = pose_frame.p[1]; + trans[2] = pose_frame.p[2]; + + KDL::Rotation mult; + KDL::Vector direction; + + switch (GetIkType()) + { + case IKP_Transform6D: + case IKP_Translation3D: + // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored. + + mult = pose_frame.M; + + double vals[9]; + vals[0] = mult(0, 0); + vals[1] = mult(0, 1); + vals[2] = mult(0, 2); + vals[3] = mult(1, 0); + vals[4] = mult(1, 1); + vals[5] = mult(1, 2); + vals[6] = mult(2, 0); + vals[7] = mult(2, 1); + vals[8] = mult(2, 2); + + // IKFast56/61 + ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_Direction3D: + case IKP_Ray4D: + case IKP_TranslationDirection5D: + // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target + // direction. + + direction = pose_frame.M * KDL::Vector(0, 0, 1); + ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationLocalGlobal6D: + // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end + // effector coordinate system. + RCLCPP_ERROR(LOGGER, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_TranslationXY2D: + ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_Rotation3D: + case IKP_Lookat3D: + case IKP_TranslationXYOrientation3D: + RCLCPP_ERROR(LOGGER, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_TranslationXAxisAngle4D: + // For *TranslationXAxisAngle4D*, *TranslationYAxisAngle4D*, *TranslationZAxisAngle4D* - end effector origin + // reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the + // manipulator base link's coordinate system) + case IKP_TranslationXAxisAngleZNorm4D: + double roll, pitch, yaw; + // For **TranslationXAxisAngleZNorm4D** - end effector origin reaches desired 3D translation, manipulator + // direction needs to be orthogonal to z axis and be rotated at a certain angle starting from the x axis (defined + // in the manipulator base link’s coordinate system) + pose_frame.M.GetRPY(roll, pitch, yaw); + ComputeIk(trans, &yaw, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationYAxisAngle4D: + case IKP_TranslationYAxisAngleXNorm4D: + // For **TranslationYAxisAngleXNorm4D** - end effector origin reaches desired 3D translation, manipulator + // direction needs to be orthogonal to x axis and be rotated at a certain angle starting from the y axis (defined + // in the manipulator base link’s coordinate system) + pose_frame.M.GetRPY(roll, pitch, yaw); + ComputeIk(trans, &roll, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationZAxisAngle4D: + case IKP_TranslationZAxisAngleYNorm4D: + // For **TranslationZAxisAngleYNorm4D** - end effector origin reaches desired 3D translation, manipulator + // direction needs to be orthogonal to y axis and be rotated at a certain angle starting from the z axis (defined + // in the manipulator base link’s coordinate system) + pose_frame.M.GetRPY(roll, pitch, yaw); + ComputeIk(trans, &pitch, vfree.size() > 0 ? &vfree[0] : nullptr, solutions); + return solutions.GetNumSolutions(); + + default: + RCLCPP_ERROR(LOGGER, "Unknown IkParameterizationType! " + "Was the solver generated with an incompatible version of Openrave?"); + return 0; + } +} + +void IKFastKinematicsPlugin::getSolution(const IkSolutionList& solutions, int i, + std::vector& solution) const +{ + solution.clear(); + solution.resize(num_joints_); + + // IKFast56/61 + const IkSolutionBase& sol = solutions.GetSolution(i); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : nullptr); + + for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) + { + if (joint_has_limits_vector_[joint_id]) + { + solution[joint_id] = enforceLimits(solution[joint_id], joint_min_vector_[joint_id], joint_max_vector_[joint_id]); + } + } +} + +void IKFastKinematicsPlugin::getSolution(const IkSolutionList& solutions, + const std::vector& ik_seed_state, int i, + std::vector& solution) const +{ + solution.clear(); + solution.resize(num_joints_); + + // IKFast56/61 + const IkSolutionBase& sol = solutions.GetSolution(i); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : nullptr); + + // rotate joints by +/-360° where it is possible and useful + for (std::size_t i = 0; i < num_joints_; ++i) + { + if (joint_has_limits_vector_[i]) + { + solution[i] = enforceLimits(solution[i], joint_min_vector_[i], joint_max_vector_[i]); + double signed_distance = solution[i] - ik_seed_state[i]; + while (signed_distance > M_PI && solution[i] - 2 * M_PI > (joint_min_vector_[i] - LIMIT_TOLERANCE)) + { + signed_distance -= 2 * M_PI; + solution[i] -= 2 * M_PI; + } + while (signed_distance < -M_PI && solution[i] + 2 * M_PI < (joint_max_vector_[i] + LIMIT_TOLERANCE)) + { + signed_distance += 2 * M_PI; + solution[i] += 2 * M_PI; + } + } + } +} + +double IKFastKinematicsPlugin::enforceLimits(double joint_value, double min, double max) const +{ + // If the joint_value is greater than max subtract 2 * PI until it is less than the max + while (joint_value > max) + { + joint_value -= 2 * M_PI; + } + + // If the joint_value is less than the min, add 2 * PI until it is more than the min + while (joint_value < min) + { + joint_value += 2 * M_PI; + } + return joint_value; +} + +void IKFastKinematicsPlugin::fillFreeParams(int count, int* array) +{ + free_params_.clear(); + for (int i = 0; i < count; ++i) + free_params_.push_back(array[i]); +} + +bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const +{ + if (count > 0) + { + if (-count >= min_count) + { + count = -count; + return true; + } + else if (count + 1 <= max_count) + { + count = count + 1; + return true; + } + else + { + return false; + } + } + else + { + if (1 - count <= max_count) + { + count = 1 - count; + return true; + } + else if (count - 1 >= min_count) + { + count = count - 1; + return true; + } + else + return false; + } +} + +bool IKFastKinematicsPlugin::getPositionFK(const std::vector& link_names, + const std::vector& joint_angles, + std::vector& poses) const +{ + if (GetIkType() != IKP_Transform6D) + { + // ComputeFk() is the inverse function of ComputeIk(), so the format of + // eerot differs depending on IK type. The Transform6D IK type is the only + // one for which a 3x3 rotation matrix is returned, which means we can only + // compute FK for that IK type. + RCLCPP_ERROR(LOGGER, "Can only compute FK for Transform6D IK type!"); + return false; + } + + KDL::Frame p_out; + if (link_names.size() == 0) + { + RCLCPP_WARN_STREAM(LOGGER, "Link names with nothing"); + return false; + } + + if (link_names.size() != 1 || link_names[0] != getTipFrame()) + { + RCLCPP_ERROR(LOGGER, "Can compute FK for %s only", getTipFrame().c_str()); + return false; + } + + bool valid = true; + + IkReal eerot[9], eetrans[3]; + + if (joint_angles.size() != num_joints_) + { + RCLCPP_ERROR(LOGGER, "Unexpected number of joint angles"); + return false; + } + + IkReal angles[num_joints_]; + for (unsigned char i = 0; i < num_joints_; ++i) + angles[i] = joint_angles[i]; + + // IKFast56/61 + ComputeFk(angles, eetrans, eerot); + + for (int i = 0; i < 3; ++i) + p_out.p.data[i] = eetrans[i]; + + for (int i = 0; i < 9; ++i) + p_out.M.data[i] = eerot[i]; + + poses.resize(1); + poses[0] = tf2::toMsg(p_out); + + return valid; +} + +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + std::vector consistency_limits; + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, + options); +} + +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, + std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, + options); +} + +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + std::vector consistency_limits; + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, + options); +} + +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + // "SEARCH_MODE" is fixed during code generation + SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT; + + // Check if there are no redundant joints + if (free_params_.size() == 0) + { + RCLCPP_DEBUG_STREAM(LOGGER, "No need to search since no free params/redundant joints"); + + std::vector ik_poses(1, ik_pose); + std::vector> solutions; + kinematics::KinematicsResult kinematic_result; + // Find all IK solutions within joint limits + if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options)) + { + RCLCPP_DEBUG(LOGGER, "No solution whatsoever"); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + return false; + } + + // sort solutions by their distance to the seed + std::vector solutions_obey_limits; + for (std::size_t i = 0; i < solutions.size(); ++i) + { + double dist_from_seed = 0.0; + for (std::size_t j = 0; j < ik_seed_state.size(); ++j) + { + dist_from_seed += fabs(ik_seed_state[j] - solutions[i][j]); + } + + solutions_obey_limits.push_back({ solutions[i], dist_from_seed }); + } + std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end()); + + // check for collisions if a callback is provided + if (solution_callback) + { + for (std::size_t i = 0; i < solutions_obey_limits.size(); ++i) + { + solution_callback(ik_pose, solutions_obey_limits[i].value, error_code); + if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + solution = solutions_obey_limits[i].value; + RCLCPP_DEBUG(LOGGER, "Solution passes callback"); + return true; + } + } + + RCLCPP_DEBUG_STREAM(LOGGER, "Solution has error code " << error_code.val); + return false; + } + else + { + solution = solutions_obey_limits[0].value; + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + return true; // no collision check callback provided + } + } + + // ------------------------------------------------------------------------------------------------- + // Error Checking + if (!initialized_) + { + RCLCPP_ERROR(LOGGER, "Kinematics not active"); + error_code.val = error_code.NO_IK_SOLUTION; + return false; + } + + if (ik_seed_state.size() != num_joints_) + { + RCLCPP_ERROR_STREAM(LOGGER, + "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); + error_code.val = error_code.NO_IK_SOLUTION; + return false; + } + + if (!consistency_limits.empty() && consistency_limits.size() != num_joints_) + { + RCLCPP_ERROR_STREAM(LOGGER, "Consistency limits be empty or must have size " << num_joints_ << " instead of size " + << consistency_limits.size()); + error_code.val = error_code.NO_IK_SOLUTION; + return false; + } + + // ------------------------------------------------------------------------------------------------- + // Initialize + + KDL::Frame frame; + transformToChainFrame(ik_pose, frame); + + std::vector vfree(free_params_.size()); + + int counter = 0; + + double initial_guess = ik_seed_state[free_params_[0]]; + vfree[0] = initial_guess; + + // ------------------------------------------------------------------------------------------------- + // Handle consistency limits if needed + int num_positive_increments; + int num_negative_increments; + double search_discretization = redundant_joint_discretization_.at(free_params_[0]); + + if (!consistency_limits.empty()) + { + // MoveIt replaced consistency_limit (scalar) w/ consistency_limits (vector) + // Assume [0]th free_params element for now. Probably wrong. + double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]); + double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]); + + num_positive_increments = static_cast((max_limit - initial_guess) / search_discretization); + num_negative_increments = static_cast((initial_guess - min_limit) / search_discretization); + } + else // no consistency limits provided + { + num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization; + num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization; + } + + // ------------------------------------------------------------------------------------------------- + // Begin searching + + RCLCPP_DEBUG_STREAM(LOGGER, "Free param is " << free_params_[0] << " initial guess is " << initial_guess + << ", # positive increments: " << num_positive_increments + << ", # negative increments: " << num_negative_increments); + if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000) + RCLCPP_WARN_STREAM_ONCE(LOGGER, "Large search space, consider increasing the search discretization"); + + double best_costs = -1.0; + std::vector best_solution; + int nattempts = 0, nvalid = 0; + + while (true) + { + IkSolutionList solutions; + size_t numsol = solve(frame, vfree, solutions); + + RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast"); + + if (numsol > 0) + { + for (size_t s = 0; s < numsol; ++s) + { + nattempts++; + std::vector sol; + getSolution(solutions, ik_seed_state, s, sol); + + bool obeys_limits = true; + for (size_t i = 0; i < sol.size(); ++i) + { + if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) + { + obeys_limits = false; + break; + } + // RCLCPP_INFO_STREAM(LOGGER,"Num " << i << " value " << sol[i] << " has limits " << + // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); + } + if (obeys_limits) + { + getSolution(solutions, ik_seed_state, s, solution); + + // This solution is within joint limits, now check if in collision (if callback provided) + if (solution_callback) + { + solution_callback(ik_pose, solution, error_code); + } + else + { + error_code.val = error_code.SUCCESS; + } + + if (error_code.val == error_code.SUCCESS) + { + nvalid++; + if (search_mode & OPTIMIZE_MAX_JOINT) + { + // Costs for solution: Largest joint motion + double costs = 0.0; + for (unsigned int i = 0; i < solution.size(); ++i) + { + double d = fabs(ik_seed_state[i] - solution[i]); + if (d > costs) + costs = d; + } + if (costs < best_costs || best_costs == -1.0) + { + best_costs = costs; + best_solution = solution; + } + } + else + // Return first feasible solution + return true; + } + } + } + } + + if (!getCount(counter, num_positive_increments, -num_negative_increments)) + { + // Everything searched + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + break; + } + + vfree[0] = initial_guess + search_discretization * counter; + // RCLCPP_DEBUG_STREAM(LOGGER,"Attempt " << counter << " with 0th free joint having value " << vfree[0]); + } + + RCLCPP_DEBUG_STREAM(LOGGER, "Valid solutions: " << nvalid << "/" << nattempts); + + if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0) + { + solution = best_solution; + error_code.val = error_code.SUCCESS; + return true; + } + + // No solution found + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + return false; +} + +// Used when there are no redundant joints - aka no free params +bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, + const std::vector& ik_seed_state, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + RCLCPP_DEBUG_STREAM(LOGGER, "getPositionIK"); + + if (!initialized_) + { + RCLCPP_ERROR(LOGGER, "kinematics not active"); + return false; + } + + if (ik_seed_state.size() < num_joints_) + { + RCLCPP_ERROR_STREAM(LOGGER, "ik_seed_state only has " << ik_seed_state.size() + << " entries, this ikfast solver requires " << num_joints_); + return false; + } + + // Check if seed is in bound + for (std::size_t i = 0; i < ik_seed_state.size(); ++i) + { + // Add tolerance to limit check + if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || + (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) + { + RCLCPP_DEBUG_STREAM(LOGGER, "IK seed not in limits! " << static_cast(i) << " value " << ik_seed_state[i] + << " has limit: " << joint_has_limits_vector_[i] + << " being " << joint_min_vector_[i] << " to " + << joint_max_vector_[i]); + return false; + } + } + + std::vector vfree(free_params_.size()); + for (std::size_t i = 0; i < free_params_.size(); ++i) + { + int p = free_params_[i]; + RCLCPP_ERROR(LOGGER, "%u is %f", p, ik_seed_state[p]); // DTC + vfree[i] = ik_seed_state[p]; + } + + KDL::Frame frame; + transformToChainFrame(ik_pose, frame); + + IkSolutionList solutions; + size_t numsol = solve(frame, vfree, solutions); + RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast"); + + std::vector solutions_obey_limits; + + if (numsol) + { + std::vector solution_obey_limits; + for (std::size_t s = 0; s < numsol; ++s) + { + std::vector sol; + getSolution(solutions, ik_seed_state, s, sol); + RCLCPP_DEBUG(LOGGER, "Sol %d: %e %e %e %e %e %e", static_cast(s), sol[0], sol[1], sol[2], sol[3], + sol[4], sol[5]); + + bool obeys_limits = true; + for (std::size_t i = 0; i < sol.size(); ++i) + { + // Add tolerance to limit check + if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || + (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) + { + // One element of solution is not within limits + obeys_limits = false; + RCLCPP_DEBUG_STREAM(LOGGER, "Not in limits! " << static_cast(i) << " value " << sol[i] + << " has limit: " << joint_has_limits_vector_[i] << " being " + << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + break; + } + } + if (obeys_limits) + { + // All elements of this solution obey limits + getSolution(solutions, ik_seed_state, s, solution_obey_limits); + double dist_from_seed = 0.0; + for (std::size_t i = 0; i < ik_seed_state.size(); ++i) + { + dist_from_seed += fabs(ik_seed_state[i] - solution_obey_limits[i]); + } + + solutions_obey_limits.push_back({ solution_obey_limits, dist_from_seed }); + } + } + } + else + { + RCLCPP_DEBUG(LOGGER, "No IK solution"); + } + + // Sort the solutions under limits and find the one that is closest to ik_seed_state + if (!solutions_obey_limits.empty()) + { + std::sort(solutions_obey_limits.begin(), solutions_obey_limits.end()); + solution = solutions_obey_limits[0].value; + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + return true; + } + + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + return false; +} + +bool IKFastKinematicsPlugin::getPositionIK(const std::vector& ik_poses, + const std::vector& ik_seed_state, + std::vector>& solutions, + kinematics::KinematicsResult& result, + const kinematics::KinematicsQueryOptions& options) const +{ + RCLCPP_DEBUG_STREAM(LOGGER, "getPositionIK with multiple solutions"); + + if (!initialized_) + { + RCLCPP_ERROR(LOGGER, "kinematics not active"); + result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE; + return false; + } + + if (ik_poses.empty()) + { + RCLCPP_ERROR(LOGGER, "ik_poses is empty"); + result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES; + return false; + } + + if (ik_poses.size() > 1) + { + RCLCPP_ERROR(LOGGER, "ik_poses contains multiple entries, only one is allowed"); + result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED; + return false; + } + + if (ik_seed_state.size() < num_joints_) + { + RCLCPP_ERROR_STREAM(LOGGER, "ik_seed_state only has " << ik_seed_state.size() + << " entries, this ikfast solver requires " << num_joints_); + return false; + } + + KDL::Frame frame; + transformToChainFrame(ik_poses[0], frame); + + // solving ik + std::vector> solution_set; + IkSolutionList ik_solutions; + std::vector vfree; + int numsol = 0; + std::vector sampled_joint_vals; + if (!redundant_joint_indices_.empty()) + { + // initializing from seed + sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]); + + // checking joint limits when using no discretization + if (options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION && + joint_has_limits_vector_[redundant_joint_indices_.front()]) + { + double joint_min = joint_min_vector_[redundant_joint_indices_.front()]; + double joint_max = joint_max_vector_[redundant_joint_indices_.front()]; + + double jv = sampled_joint_vals[0]; + if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE)))) + { + result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS; + RCLCPP_ERROR_STREAM(LOGGER, "ik seed is out of bounds"); + return false; + } + } + + // computing all solutions sets for each sampled value of the redundant joint + if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals)) + { + result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED; + return false; + } + + for (unsigned int i = 0; i < sampled_joint_vals.size(); ++i) + { + vfree.clear(); + vfree.push_back(sampled_joint_vals[i]); + numsol += solve(frame, vfree, ik_solutions); + solution_set.push_back(ik_solutions); + } + } + else + { + // computing for single solution set + numsol = solve(frame, vfree, ik_solutions); + solution_set.push_back(ik_solutions); + } + + RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast"); + bool solutions_found = false; + if (numsol > 0) + { + /* + Iterating through all solution sets and storing those that do not exceed joint limits. + */ + for (unsigned int r = 0; r < solution_set.size(); ++r) + { + ik_solutions = solution_set[r]; + numsol = ik_solutions.GetNumSolutions(); + for (int s = 0; s < numsol; ++s) + { + std::vector sol; + getSolution(ik_solutions, ik_seed_state, s, sol); + + bool obeys_limits = true; + for (unsigned int i = 0; i < sol.size(); ++i) + { + // Add tolerance to limit check + if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || + (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) + { + // One element of solution is not within limits + obeys_limits = false; + RCLCPP_DEBUG_STREAM(LOGGER, "Not in limits! " + << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] + << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + break; + } + } + if (obeys_limits) + { + // All elements of solution obey limits + solutions_found = true; + solutions.push_back(sol); + } + } + } + + if (solutions_found) + { + result.kinematic_error = kinematics::KinematicErrors::OK; + return true; + } + } + else + { + RCLCPP_DEBUG_STREAM(LOGGER, "No IK solution"); + } + + result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION; + return false; +} + +bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method, + std::vector& sampled_joint_vals) const +{ + int index = redundant_joint_indices_.front(); + double joint_dscrt = redundant_joint_discretization_.at(index); + double joint_min = joint_min_vector_[index]; + double joint_max = joint_max_vector_[index]; + + switch (method) + { + case kinematics::DiscretizationMethods::ALL_DISCRETIZED: + { + size_t steps = std::ceil((joint_max - joint_min) / joint_dscrt); + for (size_t i = 0; i < steps; ++i) + { + sampled_joint_vals.push_back(joint_min + joint_dscrt * i); + } + sampled_joint_vals.push_back(joint_max); + } + break; + case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED: + { + int steps = std::ceil((joint_max - joint_min) / joint_dscrt); + steps = steps > 0 ? steps : 1; + double diff = joint_max - joint_min; + for (int i = 0; i < steps; ++i) + { + sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast(RAND_MAX))) + joint_min); + } + } + + break; + case kinematics::DiscretizationMethods::NO_DISCRETIZATION: + + break; + default: + RCLCPP_ERROR_STREAM(LOGGER, "Discretization method " << method << " is not supported"); + return false; + } + + return true; +} + +void IKFastKinematicsPlugin::transformToChainFrame(const geometry_msgs::msg::Pose& ik_pose, + KDL::Frame& ik_pose_chain) const +{ + if (tip_transform_required_ || base_transform_required_) + { + Eigen::Isometry3d ik_eigen_pose; + tf2::fromMsg(ik_pose, ik_eigen_pose); + if (tip_transform_required_) + ik_eigen_pose = ik_eigen_pose * group_tip_to_chain_tip_; + + if (base_transform_required_) + ik_eigen_pose = chain_base_to_group_base_ * ik_eigen_pose; + + tf2::fromMsg(tf2::toMsg(ik_eigen_pose), ik_pose_chain); + } + else + { + tf2::fromMsg(ik_pose, ik_pose_chain); + } +} + +} // namespace project_perry_project_perry + +// register IKFastKinematicsPlugin as a KinematicsBase implementation +#include +PLUGINLIB_EXPORT_CLASS(project_perry_project_perry::IKFastKinematicsPlugin, kinematics::KinematicsBase); diff --git a/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_solver.cpp b/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_solver.cpp new file mode 100644 index 0000000..4260be5 --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_solver.cpp @@ -0,0 +1,5489 @@ +/// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE +/// \author Rosen Diankov +/// +/// Licensed under the Apache License, Version 2.0 (the "License"); +/// you may not use this file except in compliance with the License. +/// You may obtain a copy of the License at +/// http://www.apache.org/licenses/LICENSE-2.0 +/// +/// Unless required by applicable law or agreed to in writing, software +/// distributed under the License is distributed on an "AS IS" BASIS, +/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +/// See the License for the specific language governing permissions and +/// limitations under the License. +/// +/// ikfast version 0x10000049 generated on 2025-06-27 05:59:58.468986 +/// To compile with gcc: +/// gcc -lstdc++ ik.cpp +/// To compile without any main function as a shared object (might need -llapack): +/// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp +#define IKFAST_HAS_LIBRARY +#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h +using namespace ikfast; + +// check if the included ikfast version matches what this file was compiled with +#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] +IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049); + +#include +#include +#include +#include +#include + +#ifndef IKFAST_ASSERT +#include +#include +#include + +#ifdef _MSC_VER +#ifndef __PRETTY_FUNCTION__ +#define __PRETTY_FUNCTION__ __FUNCDNAME__ +#endif +#endif + +#ifndef __PRETTY_FUNCTION__ +#define __PRETTY_FUNCTION__ __func__ +#endif + +#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } } + +#endif + +#if defined(_MSC_VER) +#define IKFAST_ALIGNED16(x) __declspec(align(16)) x +#else +#define IKFAST_ALIGNED16(x) x __attribute((aligned(16))) +#endif + +#define IK2PI ((IkReal)6.28318530717959) +#define IKPI ((IkReal)3.14159265358979) +#define IKPI_2 ((IkReal)1.57079632679490) + +#ifdef _MSC_VER +#ifndef isnan +#define isnan _isnan +#endif +#ifndef isinf +#define isinf _isinf +#endif +//#ifndef isfinite +//#define isfinite _isfinite +//#endif +#endif // _MSC_VER + +// lapack routines +extern "C" { + void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info); + void zgetrf_ (const int* m, const int* n, std::complex* a, const int* lda, int* ipiv, int* info); + void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info); + void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info); + void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info); + void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info); +} + +using namespace std; // necessary to get std math routines + +#ifdef IKFAST_NAMESPACE +namespace IKFAST_NAMESPACE { +#endif + +inline float IKabs(float f) { return fabsf(f); } +inline double IKabs(double f) { return fabs(f); } + +inline float IKsqr(float f) { return f*f; } +inline double IKsqr(double f) { return f*f; } + +inline float IKlog(float f) { return logf(f); } +inline double IKlog(double f) { return log(f); } + +// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation +#ifndef IKFAST_SINCOS_THRESH +#define IKFAST_SINCOS_THRESH ((IkReal)1e-7) +#endif + +// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation +#ifndef IKFAST_ATAN2_MAGTHRESH +#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7) +#endif + +// minimum distance of separate solutions +#ifndef IKFAST_SOLUTION_THRESH +#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6) +#endif + +// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate +#ifndef IKFAST_EVALCOND_THRESH +#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001) +#endif + + +inline float IKasin(float f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return float(-IKPI_2); +else if( f >= 1 ) return float(IKPI_2); +return asinf(f); +} +inline double IKasin(double f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return -IKPI_2; +else if( f >= 1 ) return IKPI_2; +return asin(f); +} + +// return positive value in [0,y) +inline float IKfmod(float x, float y) +{ + while(x < 0) { + x += y; + } + return fmodf(x,y); +} + +// return positive value in [0,y) +inline double IKfmod(double x, double y) +{ + while(x < 0) { + x += y; + } + return fmod(x,y); +} + +inline float IKacos(float f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return float(IKPI); +else if( f >= 1 ) return float(0); +return acosf(f); +} +inline double IKacos(double f) +{ +IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver +if( f <= -1 ) return IKPI; +else if( f >= 1 ) return 0; +return acos(f); +} +inline float IKsin(float f) { return sinf(f); } +inline double IKsin(double f) { return sin(f); } +inline float IKcos(float f) { return cosf(f); } +inline double IKcos(double f) { return cos(f); } +inline float IKtan(float f) { return tanf(f); } +inline double IKtan(double f) { return tan(f); } +inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); } +inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); } +inline float IKatan2Simple(float fy, float fx) { + return atan2f(fy,fx); +} +inline float IKatan2(float fy, float fx) { + if( isnan(fy) ) { + IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned + return float(IKPI_2); + } + else if( isnan(fx) ) { + return 0; + } + return atan2f(fy,fx); +} +inline double IKatan2Simple(double fy, double fx) { + return atan2(fy,fx); +} +inline double IKatan2(double fy, double fx) { + if( isnan(fy) ) { + IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned + return IKPI_2; + } + else if( isnan(fx) ) { + return 0; + } + return atan2(fy,fx); +} + +template +struct CheckValue +{ + T value; + bool valid; +}; + +template +inline CheckValue IKatan2WithCheck(T fy, T fx, T epsilon) +{ + CheckValue ret; + ret.valid = false; + ret.value = 0; + if( !isnan(fy) && !isnan(fx) ) { + if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) { + ret.value = IKatan2Simple(fy,fx); + ret.valid = true; + } + } + return ret; +} + +inline float IKsign(float f) { + if( f > 0 ) { + return float(1); + } + else if( f < 0 ) { + return float(-1); + } + return 0; +} + +inline double IKsign(double f) { + if( f > 0 ) { + return 1.0; + } + else if( f < 0 ) { + return -1.0; + } + return 0; +} + +template +inline CheckValue IKPowWithIntegerCheck(T f, int n) +{ + CheckValue ret; + ret.valid = true; + if( n == 0 ) { + ret.value = 1.0; + return ret; + } + else if( n == 1 ) + { + ret.value = f; + return ret; + } + else if( n < 0 ) + { + if( f == 0 ) + { + ret.valid = false; + ret.value = (T)1.0e30; + return ret; + } + if( n == -1 ) { + ret.value = T(1.0)/f; + return ret; + } + } + + int num = n > 0 ? n : -n; + if( num == 2 ) { + ret.value = f*f; + } + else if( num == 3 ) { + ret.value = f*f*f; + } + else { + ret.value = 1.0; + while(num>0) { + if( num & 1 ) { + ret.value *= f; + } + num >>= 1; + f *= f; + } + } + + if( n < 0 ) { + ret.value = T(1.0)/ret.value; + } + return ret; +} + +/// solves the forward kinematics equations. +/// \param pfree is an array specifying the free joints of the chain. +IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) { +IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9; +x0=IKcos(j[0]); +x1=IKsin(j[1]); +x2=IKcos(j[2]); +x3=IKsin(j[0]); +x4=IKcos(j[1]); +x5=IKsin(j[2]); +x6=((0.9)*x2); +x7=(x0*x1); +x8=(x1*x3); +x9=((0.9)*x4*x5); +eetrans[0]=(((x0*x9))+x7+((x6*x7))); +eetrans[1]=(x8+((x3*x9))+((x6*x8))); +eetrans[2]=((0.75)+(((-0.9)*x1*x5))+x4+((x4*x6))); +} + +IKFAST_API int GetNumFreeParameters() { return 0; } +IKFAST_API int* GetFreeParameters() { return NULL; } +IKFAST_API int GetNumJoints() { return 3; } + +IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } + +IKFAST_API int GetIkType() { return 0x33000003; } + +class IKSolver { +public: +IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp; +unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2; + +IkReal j100, cj100, sj100; +unsigned char _ij100[2], _nj100; +bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions) { +j0=numeric_limits::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; +for(int dummyiter = 0; dummyiter < 1; ++dummyiter) { + solutions.Clear(); +px = eetrans[0]; py = eetrans[1]; pz = eetrans[2]; + +new_px=px; +new_py=py; +new_pz=((-0.75)+pz); +px = new_px; py = new_py; pz = new_pz; +pp=((px*px)+(py*py)+(pz*pz)); +{ +IkReal j2array[2], cj2array[2], sj2array[2]; +bool j2valid[2]={false}; +_nj2 = 2; +cj2array[0]=((-1.00555555555556)+(((0.555555555555556)*(pz*pz)))+(((0.555555555555556)*(py*py)))+(((0.555555555555556)*(px*px)))); +if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j2valid[0] = j2valid[1] = true; + j2array[0] = IKacos(cj2array[0]); + sj2array[0] = IKsin(j2array[0]); + cj2array[1] = cj2array[0]; + j2array[1] = -j2array[0]; + sj2array[1] = -sj2array[0]; +} +else if( isnan(cj2array[0]) ) +{ + // probably any value will work + j2valid[0] = true; + cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0; +} +for(int ij2 = 0; ij2 < 2; ++ij2) +{ +if( !j2valid[ij2] ) +{ + continue; +} +_ij2[0] = ij2; _ij2[1] = -1; +for(int iij2 = ij2+1; iij2 < 2; ++iij2) +{ +if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH ) +{ + j2valid[iij2]=false; _ij2[1] = iij2; break; +} +} +j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2]; + +{ +IkReal j0eval[1]; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j1eval[2]; +j1eval[0]=((((1.11111111111111)*(IKabs(((-1.0)+(((-0.9)*cj2)))))))+(IKabs(sj2))); +j1eval[1]=((1.23456790123457)+(sj2*sj2)+(cj2*cj2)+(((2.22222222222222)*cj2))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j1eval[1]; +px=0; +py=0; +pp=pz*pz; +j1eval[0]=pz; +if( IKabs(j1eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j1eval[2]; +px=0; +py=0; +pp=pz*pz; +j1eval[0]=pz; +j1eval[1]=((((-1.0)*cj2*pz))+(((-1.11111111111111)*pz))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=IKabs(pz); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j1eval[1]; +px=0; +py=0; +pp=0; +pz=0; +j1eval[0]=1.0; +if( IKabs(j1eval[0]) < 0.0000000100000000 ) +{ +continue; // no branches [j0, j1] + +} else +{ +IkReal op[2+1], zeror[2]; +int numroots; +op[0]=1.0; +op[1]=0; +op[2]=-1.0; +polyroots2(op,zeror,numroots); +IkReal j1array[2], cj1array[2], sj1array[2], tempj1array[1]; +int numsolutions = 0; +for(int ij1 = 0; ij1 < numroots; ++ij1) +{ +IkReal htj1 = zeror[ij1]; +tempj1array[0]=((2.0)*(atan(htj1))); +for(int kj1 = 0; kj1 < 1; ++kj1) +{ +j1array[numsolutions] = tempj1array[kj1]; +if( j1array[numsolutions] > IKPI ) +{ + j1array[numsolutions]-=IK2PI; +} +else if( j1array[numsolutions] < -IKPI ) +{ + j1array[numsolutions]+=IK2PI; +} +sj1array[numsolutions] = IKsin(j1array[numsolutions]); +cj1array[numsolutions] = IKcos(j1array[numsolutions]); +numsolutions++; +} +} +bool j1valid[2]={true,true}; +_nj1 = 2; +for(int ij1 = 0; ij1 < numsolutions; ++ij1) + { +if( !j1valid[ij1] ) +{ + continue; +} + j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; +htj1 = IKtan(j1/2); + +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + } + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0, j1] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +CheckValue x10=IKPowWithIntegerCheck(pz,-1); +if(!x10.valid){ +continue; +} +CheckValue x11=IKPowWithIntegerCheck(((((-100.0)*pz))+(((-90.0)*cj2*pz))),-1); +if(!x11.valid){ +continue; +} +if( IKabs(((-0.9)*sj2*(x10.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x11.value)*(((81.0)+(((-81.0)*(cj2*cj2)))+(((-100.0)*(pz*pz))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-0.9)*sj2*(x10.value)))+IKsqr(((x11.value)*(((81.0)+(((-81.0)*(cj2*cj2)))+(((-100.0)*(pz*pz)))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j1array[0]=IKatan2(((-0.9)*sj2*(x10.value)), ((x11.value)*(((81.0)+(((-81.0)*(cj2*cj2)))+(((-100.0)*(pz*pz))))))); +sj1array[0]=IKsin(j1array[0]); +cj1array[0]=IKcos(j1array[0]); +if( j1array[0] > IKPI ) +{ + j1array[0]-=IK2PI; +} +else if( j1array[0] < -IKPI ) +{ j1array[0]+=IK2PI; +} +j1valid[0] = true; +for(int ij1 = 0; ij1 < 1; ++ij1) +{ +if( !j1valid[ij1] ) +{ + continue; +} +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < 1; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; +{ +IkReal evalcond[5]; +IkReal x12=IKcos(j1); +IkReal x13=IKsin(j1); +IkReal x14=((0.9)*sj2); +IkReal x15=((0.9)*cj2); +IkReal x16=(pz*x12); +evalcond[0]=(((pz*x13))+x14); +evalcond[1]=((-1.0)+(((-1.0)*x15))+x16); +evalcond[2]=((-0.19)+(((2.0)*x16))+(((-1.0)*(pz*pz)))); +evalcond[3]=(x13+((x12*x14))+((x13*x15))); +evalcond[4]=((((-1.0)*x12))+pz+((x13*x14))+(((-1.0)*x12*x15))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} +} +} + +} + +} + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +CheckValue x18=IKPowWithIntegerCheck(pz,-1); +if(!x18.valid){ +continue; +} +IkReal x17=x18.value; +if( IKabs(((-0.9)*sj2*x17)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x17*(((1.0)+(((0.9)*cj2)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-0.9)*sj2*x17))+IKsqr((x17*(((1.0)+(((0.9)*cj2))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j1array[0]=IKatan2(((-0.9)*sj2*x17), (x17*(((1.0)+(((0.9)*cj2)))))); +sj1array[0]=IKsin(j1array[0]); +cj1array[0]=IKcos(j1array[0]); +if( j1array[0] > IKPI ) +{ + j1array[0]-=IK2PI; +} +else if( j1array[0] < -IKPI ) +{ j1array[0]+=IK2PI; +} +j1valid[0] = true; +for(int ij1 = 0; ij1 < 1; ++ij1) +{ +if( !j1valid[ij1] ) +{ + continue; +} +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < 1; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; +{ +IkReal evalcond[5]; +IkReal x19=IKcos(j1); +IkReal x20=IKsin(j1); +IkReal x21=((0.9)*sj2); +IkReal x22=((0.9)*cj2); +IkReal x23=(pz*x19); +evalcond[0]=(((pz*x20))+x21); +evalcond[1]=((-1.0)+(((-1.0)*x22))+x23); +evalcond[2]=((-0.19)+(((2.0)*x23))+(((-1.0)*(pz*pz)))); +evalcond[3]=(((x19*x21))+x20+((x20*x22))); +evalcond[4]=((((-1.0)*x19*x22))+(((-1.0)*x19))+pz+((x20*x21))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0, j1] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j1array[2], cj1array[2], sj1array[2]; +bool j1valid[2]={false}; +_nj1 = 2; +IkReal x24=((-1.0)+(((-0.9)*cj2))); +CheckValue x27 = IKatan2WithCheck(IkReal(x24),IkReal(((0.9)*sj2)),IKFAST_ATAN2_MAGTHRESH); +if(!x27.valid){ +continue; +} +IkReal x25=((1.0)*(x27.value)); +if((((x24*x24)+(((0.81)*(sj2*sj2))))) < -0.00001) +continue; +CheckValue x28=IKPowWithIntegerCheck(IKabs(IKsqrt(((x24*x24)+(((0.81)*(sj2*sj2)))))),-1); +if(!x28.valid){ +continue; +} +if( ((pz*(x28.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x28.value))) > 1+IKFAST_SINCOS_THRESH ) + continue; +IkReal x26=IKasin((pz*(x28.value))); +j1array[0]=((((-1.0)*x25))+(((-1.0)*x26))); +sj1array[0]=IKsin(j1array[0]); +cj1array[0]=IKcos(j1array[0]); +j1array[1]=((3.14159265358979)+(((-1.0)*x25))+x26); +sj1array[1]=IKsin(j1array[1]); +cj1array[1]=IKcos(j1array[1]); +if( j1array[0] > IKPI ) +{ + j1array[0]-=IK2PI; +} +else if( j1array[0] < -IKPI ) +{ j1array[0]+=IK2PI; +} +j1valid[0] = true; +if( j1array[1] > IKPI ) +{ + j1array[1]-=IK2PI; +} +else if( j1array[1] < -IKPI ) +{ j1array[1]+=IK2PI; +} +j1valid[1] = true; +for(int ij1 = 0; ij1 < 2; ++ij1) +{ +if( !j1valid[ij1] ) +{ + continue; +} +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < 2; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; + +{ +IkReal j0eval[3]; +IkReal x29=((9.0)*sj2); +IkReal x30=((10.0)*pz*sj1); +IkReal x31=(((cj1*(px*px)))+((cj1*(py*py)))); +j0eval[0]=x31; +j0eval[1]=((IKabs((((px*x29))+((px*x30)))))+(IKabs((((py*x29))+((py*x30)))))); +j0eval[2]=IKsign(x31); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +IkReal x32=((9.0)*cj2); +IkReal x33=((10.0)*cj1*pz); +IkReal x34=(((sj1*(py*py)))+((sj1*(px*px)))); +j0eval[0]=x34; +j0eval[1]=((IKabs((((py*x32))+(((-1.0)*py*x33))+(((10.0)*py)))))+(IKabs(((((10.0)*px))+((px*x32))+(((-1.0)*px*x33)))))); +j0eval[2]=IKsign(x34); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +IkReal x35=((9.0)*px); +IkReal x36=(cj1*sj2); +IkReal x37=(cj2*sj1); +IkReal x38=((10.0)*sj1); +IkReal x39=((9.0)*py); +IkReal x40=((px*px)+(py*py)); +j0eval[0]=x40; +j0eval[1]=IKsign(x40); +j0eval[2]=((IKabs((((x35*x37))+((x35*x36))+((px*x38)))))+(IKabs((((x37*x39))+((py*x38))+((x36*x39)))))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((px*px)+(py*py)); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j1))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[4]; +sj1=0; +cj1=1.0; +j1=0; +IkReal x41=px*px; +IkReal x42=py*py; +j0eval[0]=(x42+x41); +j0eval[1]=81.0; +j0eval[2]=sj2; +j0eval[3]=IKsign(((((10.0)*x41))+(((10.0)*x42)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 || IKabs(j0eval[3]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((px*px)+(py*py)); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j2))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +sj1=0; +cj1=1.0; +j1=0; +sj2=0; +cj2=1.0; +j2=0; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[4], cj0array[4], sj0array[4]; +bool j0valid[4]={false}; +_nj0 = 4; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=1.5707963267949; +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +j0array[2]=3.14159265358979; +sj0array[2]=IKsin(j0array[2]); +cj0array[2]=IKcos(j0array[2]); +j0array[3]=-1.5707963267949; +sj0array[3]=IKsin(j0array[3]); +cj0array[3]=IKcos(j0array[3]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +if( j0array[2] > IKPI ) +{ + j0array[2]-=IK2PI; +} +else if( j0array[2] < -IKPI ) +{ j0array[2]+=IK2PI; +} +j0valid[2] = true; +if( j0array[3] > IKPI ) +{ + j0array[3]-=IK2PI; +} +else if( j0array[3] < -IKPI ) +{ j0array[3]+=IK2PI; +} +j0valid[3] = true; +for(int ij0 = 0; ij0 < 4; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 4; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x44 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x44.valid){ +continue; +} +IkReal x43=x44.value; +j0array[0]=((-1.0)*x43); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x43))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*py*(IKsin(j0))))+(((-1.0)*px*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j2)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +sj1=0; +cj1=1.0; +j1=0; +sj2=0; +cj2=-1.0; +j2=3.14159265358979; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[4], cj0array[4], sj0array[4]; +bool j0valid[4]={false}; +_nj0 = 4; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=1.5707963267949; +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +j0array[2]=3.14159265358979; +sj0array[2]=IKsin(j0array[2]); +cj0array[2]=IKcos(j0array[2]); +j0array[3]=-1.5707963267949; +sj0array[3]=IKsin(j0array[3]); +cj0array[3]=IKcos(j0array[3]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +if( j0array[2] > IKPI ) +{ + j0array[2]-=IK2PI; +} +else if( j0array[2] < -IKPI ) +{ j0array[2]+=IK2PI; +} +j0valid[2] = true; +if( j0array[3] > IKPI ) +{ + j0array[3]-=IK2PI; +} +else if( j0array[3] < -IKPI ) +{ j0array[3]+=IK2PI; +} +j0valid[3] = true; +for(int ij0 = 0; ij0 < 4; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 4; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x46 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x46.valid){ +continue; +} +IkReal x45=x46.value; +j0array[0]=((-1.0)*x45); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x45))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*py*(IKsin(j0))))+(((-1.0)*px*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x47=((9.0)*sj2); +CheckValue x48=IKPowWithIntegerCheck(IKsign(((((10.0)*(px*px)))+(((10.0)*(py*py))))),-1); +if(!x48.valid){ +continue; +} +CheckValue x49 = IKatan2WithCheck(IkReal((py*x47)),IkReal((px*x47)),IKFAST_ATAN2_MAGTHRESH); +if(!x49.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x48.value)))+(x49.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[2]; +IkReal x50=IKcos(j0); +IkReal x51=IKsin(j0); +IkReal x52=((1.0)*py); +evalcond[0]=(((px*x51))+(((-1.0)*x50*x52))); +evalcond[1]=((((-1.0)*x51*x52))+(((0.9)*sj2))+(((-1.0)*px*x50))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j1)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[4]; +sj1=0; +cj1=-1.0; +j1=3.14159265358979; +IkReal x53=px*px; +IkReal x54=py*py; +j0eval[0]=((((-1.0)*x53))+(((-1.0)*x54))); +j0eval[1]=81.0; +j0eval[2]=sj2; +j0eval[3]=IKsign(((((-10.0)*x53))+(((-10.0)*x54)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 || IKabs(j0eval[3]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j2))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +sj1=0; +cj1=-1.0; +j1=3.14159265358979; +sj2=0; +cj2=1.0; +j2=0; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[4], cj0array[4], sj0array[4]; +bool j0valid[4]={false}; +_nj0 = 4; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=1.5707963267949; +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +j0array[2]=3.14159265358979; +sj0array[2]=IKsin(j0array[2]); +cj0array[2]=IKcos(j0array[2]); +j0array[3]=-1.5707963267949; +sj0array[3]=IKsin(j0array[3]); +cj0array[3]=IKcos(j0array[3]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +if( j0array[2] > IKPI ) +{ + j0array[2]-=IK2PI; +} +else if( j0array[2] < -IKPI ) +{ j0array[2]+=IK2PI; +} +j0valid[2] = true; +if( j0array[3] > IKPI ) +{ + j0array[3]-=IK2PI; +} +else if( j0array[3] < -IKPI ) +{ j0array[3]+=IK2PI; +} +j0valid[3] = true; +for(int ij0 = 0; ij0 < 4; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 4; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x56 = IKatan2WithCheck(IkReal(px),IkReal(py),IKFAST_ATAN2_MAGTHRESH); +if(!x56.valid){ +continue; +} +IkReal x55=x56.value; +j0array[0]=((-1.0)*x55); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x55))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=(((px*(IKsin(j0))))+(((-1.0)*py*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j2)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +sj1=0; +cj1=-1.0; +j1=3.14159265358979; +sj2=0; +cj2=-1.0; +j2=3.14159265358979; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[4], cj0array[4], sj0array[4]; +bool j0valid[4]={false}; +_nj0 = 4; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=1.5707963267949; +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +j0array[2]=3.14159265358979; +sj0array[2]=IKsin(j0array[2]); +cj0array[2]=IKcos(j0array[2]); +j0array[3]=-1.5707963267949; +sj0array[3]=IKsin(j0array[3]); +cj0array[3]=IKcos(j0array[3]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +if( j0array[2] > IKPI ) +{ + j0array[2]-=IK2PI; +} +else if( j0array[2] < -IKPI ) +{ j0array[2]+=IK2PI; +} +j0valid[2] = true; +if( j0array[3] > IKPI ) +{ + j0array[3]-=IK2PI; +} +else if( j0array[3] < -IKPI ) +{ j0array[3]+=IK2PI; +} +j0valid[3] = true; +for(int ij0 = 0; ij0 < 4; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 4; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x58 = IKatan2WithCheck(IkReal(px),IkReal(py),IKFAST_ATAN2_MAGTHRESH); +if(!x58.valid){ +continue; +} +IkReal x57=x58.value; +j0array[0]=((-1.0)*x57); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x57))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=(((px*(IKsin(j0))))+(((-1.0)*py*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x59=((9.0)*sj2); +CheckValue x60=IKPowWithIntegerCheck(IKsign(((((-10.0)*(py*py)))+(((-10.0)*(px*px))))),-1); +if(!x60.valid){ +continue; +} +CheckValue x61 = IKatan2WithCheck(IkReal((py*x59)),IkReal((px*x59)),IKFAST_ATAN2_MAGTHRESH); +if(!x61.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x60.value)))+(x61.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[2]; +IkReal x62=IKsin(j0); +IkReal x63=IKcos(j0); +evalcond[0]=(((px*x62))+(((-1.0)*py*x63))); +evalcond[1]=((((0.9)*sj2))+((px*x63))+((py*x62))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j1)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[3]; +sj1=1.0; +cj1=0; +j1=1.5707963267949; +IkReal x64=((9.0)*cj2); +IkReal x65=((((-1.0)*(px*px)))+(((-1.0)*(py*py)))); +j0eval[0]=x65; +j0eval[1]=((IKabs(((((-10.0)*py))+(((-1.0)*py*x64)))))+(IKabs(((((-1.0)*px*x64))+(((-10.0)*px)))))); +j0eval[2]=IKsign(x65); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +sj1=1.0; +cj1=0; +j1=1.5707963267949; +IkReal x66=px*px; +IkReal x67=py*py; +IkReal x68=pz*pz; +IkReal x69=((100.0)*px); +IkReal x70=((100.0)*py); +j0eval[0]=((((-1.0)*x67))+(((-1.0)*x66))); +j0eval[1]=((IKabs(((((-1.0)*x67*x69))+(((-19.0)*px))+(((-1.0)*x68*x69))+(((-1.0)*x69*(px*px))))))+(IKabs(((((-1.0)*x66*x70))+(((-19.0)*py))+(((-1.0)*x70*(py*py)))+(((-1.0)*x68*x70)))))); +j0eval[2]=IKsign(((((-2.0)*x67))+(((-2.0)*x66)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x71=px*px; +IkReal x72=py*py; +IkReal x73=pz*pz; +IkReal x74=((1.0)*x73); +CheckValue x75 = IKatan2WithCheck(IkReal(((((-1.0)*(py*py*py)))+(((-0.19)*py))+(((-1.0)*py*x74))+(((-1.0)*py*x71)))),IkReal(((((-1.0)*px*x74))+(((-1.0)*px*x72))+(((-0.19)*px))+(((-1.0)*(px*px*px))))),IKFAST_ATAN2_MAGTHRESH); +if(!x75.valid){ +continue; +} +CheckValue x76=IKPowWithIntegerCheck(IKsign(((((-2.0)*x72))+(((-2.0)*x71)))),-1); +if(!x76.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x75.value)+(((1.5707963267949)*(x76.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[3]; +IkReal x77=IKsin(j0); +IkReal x78=IKcos(j0); +IkReal x79=(py*x77); +IkReal x80=(px*x78); +evalcond[0]=(((px*x77))+(((-1.0)*py*x78))); +evalcond[1]=((-1.0)+(((-0.9)*cj2))+x79+x80); +evalcond[2]=((-0.19)+(((-1.0)*(px*px)))+(((2.0)*x80))+(((-1.0)*(pz*pz)))+(((2.0)*x79))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x81=((0.9)*cj2); +CheckValue x82=IKPowWithIntegerCheck(IKsign(((((-1.0)*(px*px)))+(((-1.0)*(py*py))))),-1); +if(!x82.valid){ +continue; +} +CheckValue x83 = IKatan2WithCheck(IkReal(((((-1.0)*py*x81))+(((-1.0)*py)))),IkReal(((((-1.0)*px))+(((-1.0)*px*x81)))),IKFAST_ATAN2_MAGTHRESH); +if(!x83.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x82.value)))+(x83.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[3]; +IkReal x84=IKsin(j0); +IkReal x85=IKcos(j0); +IkReal x86=(py*x84); +IkReal x87=(px*x85); +evalcond[0]=((((-1.0)*py*x85))+((px*x84))); +evalcond[1]=((-1.0)+(((-0.9)*cj2))+x86+x87); +evalcond[2]=((-0.19)+(((-1.0)*(px*px)))+(((2.0)*x86))+(((2.0)*x87))+(((-1.0)*(pz*pz)))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j1)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[3]; +sj1=-1.0; +cj1=0; +j1=-1.5707963267949; +IkReal x88=((9.0)*cj2); +IkReal x89=((px*px)+(py*py)); +j0eval[0]=x89; +j0eval[1]=IKsign(x89); +j0eval[2]=((IKabs(((((-10.0)*px))+(((-1.0)*px*x88)))))+(IKabs(((((-1.0)*py*x88))+(((-10.0)*py)))))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +sj1=-1.0; +cj1=0; +j1=-1.5707963267949; +IkReal x90=px*px; +IkReal x91=py*py; +IkReal x92=pz*pz; +IkReal x93=((100.0)*px); +IkReal x94=((100.0)*py); +j0eval[0]=(x91+x90); +j0eval[1]=((IKabs(((((-1.0)*x93*(px*px)))+(((-1.0)*x91*x93))+(((-19.0)*px))+(((-1.0)*x92*x93)))))+(IKabs(((((-1.0)*x90*x94))+(((-19.0)*py))+(((-1.0)*x92*x94))+(((-1.0)*x94*(py*py))))))); +j0eval[2]=IKsign(((((2.0)*x91))+(((2.0)*x90)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((px*px)+(py*py)); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x95=px*px; +IkReal x96=py*py; +IkReal x97=pz*pz; +IkReal x98=((1.0)*x97); +CheckValue x99=IKPowWithIntegerCheck(IKsign(((((2.0)*x95))+(((2.0)*x96)))),-1); +if(!x99.valid){ +continue; +} +CheckValue x100 = IKatan2WithCheck(IkReal(((((-1.0)*(py*py*py)))+(((-1.0)*py*x95))+(((-0.19)*py))+(((-1.0)*py*x98)))),IkReal(((((-1.0)*px*x96))+(((-0.19)*px))+(((-1.0)*(px*px*px)))+(((-1.0)*px*x98)))),IKFAST_ATAN2_MAGTHRESH); +if(!x100.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x99.value)))+(x100.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[3]; +IkReal x101=IKcos(j0); +IkReal x102=IKsin(j0); +IkReal x103=((1.0)*x101); +IkReal x104=(py*x102); +evalcond[0]=(((px*x102))+(((-1.0)*py*x103))); +evalcond[1]=((-1.0)+(((-1.0)*px*x103))+(((-0.9)*cj2))+(((-1.0)*x104))); +evalcond[2]=((-0.19)+(((-1.0)*(px*px)))+(((-2.0)*x104))+(((-1.0)*(pz*pz)))+(((-2.0)*px*x101))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x105=((0.9)*cj2); +CheckValue x106 = IKatan2WithCheck(IkReal(((((-1.0)*py))+(((-1.0)*py*x105)))),IkReal(((((-1.0)*px*x105))+(((-1.0)*px)))),IKFAST_ATAN2_MAGTHRESH); +if(!x106.valid){ +continue; +} +CheckValue x107=IKPowWithIntegerCheck(IKsign(((px*px)+(py*py))),-1); +if(!x107.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x106.value)+(((1.5707963267949)*(x107.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[3]; +IkReal x108=IKcos(j0); +IkReal x109=IKsin(j0); +IkReal x110=((1.0)*x108); +IkReal x111=(py*x109); +evalcond[0]=(((px*x109))+(((-1.0)*py*x110))); +evalcond[1]=((-1.0)+(((-1.0)*px*x110))+(((-0.9)*cj2))+(((-1.0)*x111))); +evalcond[2]=((-0.19)+(((-1.0)*(px*px)))+(((-2.0)*x111))+(((-1.0)*(pz*pz)))+(((-2.0)*px*x108))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(pz))+(IKabs(((-3.14159265358979)+(IKfmod(((3.14159265358979)+j2), 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[3]; +pz=0; +j2=0; +sj2=0; +cj2=1.0; +pp=((px*px)+(py*py)); +IkReal x112=px*px; +IkReal x113=py*py; +IkReal x114=(sj1*x112); +IkReal x115=(sj1*x113); +j0eval[0]=(x115+x114); +j0eval[1]=((IKabs(px))+(IKabs(py))); +j0eval[2]=IKsign(((((10.0)*x115))+(((10.0)*x114)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[4]; +pz=0; +j2=0; +sj2=0; +cj2=1.0; +pp=((px*px)+(py*py)); +IkReal x116=px*px; +IkReal x117=py*py; +j0eval[0]=(x117+x116); +j0eval[1]=361.0; +j0eval[2]=sj1; +j0eval[3]=IKsign(((((10.0)*x117))+(((10.0)*x116)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 || IKabs(j0eval[3]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +pz=0; +j2=0; +sj2=0; +cj2=1.0; +pp=((px*px)+(py*py)); +IkReal x118=py*py; +IkReal x119=px*px; +IkReal x120=(sj1*x119); +IkReal x121=(sj1*x118); +j0eval[0]=(x120+x121); +j0eval[1]=((IKabs(((((19.0)*px))+(((100.0)*(px*px*px)))+(((100.0)*px*x118)))))+(IKabs(((((19.0)*py))+(((100.0)*py*x119))+(((100.0)*(py*py*py))))))); +j0eval[2]=IKsign(((((2.0)*x121))+(((2.0)*x120)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j1))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +pz=0; +j2=0; +sj2=0; +cj2=1.0; +pp=((px*px)+(py*py)); +sj1=0; +cj1=1.0; +j1=0; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x123 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x123.valid){ +continue; +} +IkReal x122=x123.value; +j0array[0]=((-1.0)*x122); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x122))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*py*(IKsin(j0))))+(((-1.0)*px*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j1)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +pz=0; +j2=0; +sj2=0; +cj2=1.0; +pp=((px*px)+(py*py)); +sj1=0; +cj1=-1.0; +j1=3.14159265358979; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x125 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x125.valid){ +continue; +} +IkReal x124=x125.value; +j0array[0]=((-1.0)*x124); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x124))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*py*(IKsin(j0))))+(((-1.0)*px*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((px*px)+(py*py)); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x126=py*py; +IkReal x127=px*px; +IkReal x128=((2.0)*sj1); +CheckValue x129 = IKatan2WithCheck(IkReal(((((0.19)*py))+((py*x127))+(py*py*py))),IkReal(((((0.19)*px))+(px*px*px)+((px*x126)))),IKFAST_ATAN2_MAGTHRESH); +if(!x129.valid){ +continue; +} +CheckValue x130=IKPowWithIntegerCheck(IKsign((((x126*x128))+((x127*x128)))),-1); +if(!x130.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x129.value)+(((1.5707963267949)*(x130.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[4]; +IkReal x131=IKcos(j0); +IkReal x132=IKsin(j0); +IkReal x133=((2.0)*sj1); +IkReal x134=(px*x131); +IkReal x135=(py*x132); +evalcond[0]=(((px*x132))+(((-1.0)*py*x131))); +evalcond[1]=((-1.9)+((sj1*x134))+((sj1*x135))); +evalcond[2]=((((1.9)*sj1))+(((-1.0)*x134))+(((-1.0)*x135))); +evalcond[3]=((-0.19)+(((-1.0)*(px*px)))+((x133*x134))+((x133*x135))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x136=((19.0)*sj1); +CheckValue x137 = IKatan2WithCheck(IkReal((py*x136)),IkReal((px*x136)),IKFAST_ATAN2_MAGTHRESH); +if(!x137.valid){ +continue; +} +CheckValue x138=IKPowWithIntegerCheck(IKsign(((((10.0)*(px*px)))+(((10.0)*(py*py))))),-1); +if(!x138.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x137.value)+(((1.5707963267949)*(x138.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[4]; +IkReal x139=IKcos(j0); +IkReal x140=IKsin(j0); +IkReal x141=((2.0)*sj1); +IkReal x142=(px*x139); +IkReal x143=(py*x140); +evalcond[0]=((((-1.0)*py*x139))+((px*x140))); +evalcond[1]=((-1.9)+((sj1*x143))+((sj1*x142))); +evalcond[2]=((((1.9)*sj1))+(((-1.0)*x142))+(((-1.0)*x143))); +evalcond[3]=((-0.19)+(((-1.0)*(px*px)))+((x141*x143))+((x141*x142))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x144=((10.0)*sj1); +CheckValue x145 = IKatan2WithCheck(IkReal(((19.0)*py)),IkReal(((19.0)*px)),IKFAST_ATAN2_MAGTHRESH); +if(!x145.valid){ +continue; +} +CheckValue x146=IKPowWithIntegerCheck(IKsign((((x144*(py*py)))+((x144*(px*px))))),-1); +if(!x146.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x145.value)+(((1.5707963267949)*(x146.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[4]; +IkReal x147=IKcos(j0); +IkReal x148=IKsin(j0); +IkReal x149=((2.0)*sj1); +IkReal x150=(px*x147); +IkReal x151=(py*x148); +evalcond[0]=((((-1.0)*py*x147))+((px*x148))); +evalcond[1]=((-1.9)+((sj1*x150))+((sj1*x151))); +evalcond[2]=((((1.9)*sj1))+(((-1.0)*x151))+(((-1.0)*x150))); +evalcond[3]=((-0.19)+((x149*x150))+((x149*x151))+(((-1.0)*(px*px)))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(pz))+(IKabs(((-3.14159265358979)+(IKfmod(j2, 6.28318530717959)))))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[3]; +pz=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +pp=((px*px)+(py*py)); +IkReal x152=px*px; +IkReal x153=py*py; +IkReal x154=(sj1*x152); +IkReal x155=(sj1*x153); +j0eval[0]=(x155+x154); +j0eval[1]=((IKabs(px))+(IKabs(py))); +j0eval[2]=IKsign(((((10.0)*x155))+(((10.0)*x154)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +pz=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +pp=((px*px)+(py*py)); +IkReal x156=px*px; +IkReal x157=py*py; +j0eval[0]=(x157+x156); +j0eval[1]=((IKabs((py*sj1)))+(IKabs((px*sj1)))); +j0eval[2]=IKsign(((((10.0)*x156))+(((10.0)*x157)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j0eval[3]; +pz=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +pp=((px*px)+(py*py)); +IkReal x158=py*py; +IkReal x159=px*px; +IkReal x160=(sj1*x159); +IkReal x161=(sj1*x158); +j0eval[0]=(x160+x161); +j0eval[1]=((IKabs(((((19.0)*px))+(((100.0)*px*x158))+(((100.0)*(px*px*px))))))+(IKabs(((((100.0)*py*x159))+(((19.0)*py))+(((100.0)*(py*py*py))))))); +j0eval[2]=IKsign(((((2.0)*x161))+(((2.0)*x160)))); +if( IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 || IKabs(j0eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j1))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +pz=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +pp=((px*px)+(py*py)); +sj1=0; +cj1=1.0; +j1=0; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x163 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x163.valid){ +continue; +} +IkReal x162=x163.value; +j0array[0]=((-1.0)*x162); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x162))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*py*(IKsin(j0))))+(((-1.0)*px*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j1)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0eval[1]; +pz=0; +j2=3.14159265358979; +sj2=0; +cj2=-1.0; +pp=((px*px)+(py*py)); +sj1=0; +cj1=-1.0; +j1=3.14159265358979; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x165 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x165.valid){ +continue; +} +IkReal x164=x165.value; +j0array[0]=((-1.0)*x164); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x164))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*py*(IKsin(j0))))+(((-1.0)*px*(IKcos(j0))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((px*px)+(py*py)); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +j0array[0]=0; +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x166=py*py; +IkReal x167=px*px; +IkReal x168=((2.0)*sj1); +CheckValue x169=IKPowWithIntegerCheck(IKsign((((x167*x168))+((x166*x168)))),-1); +if(!x169.valid){ +continue; +} +CheckValue x170 = IKatan2WithCheck(IkReal((((py*x167))+(((0.19)*py))+(py*py*py))),IkReal(((((0.19)*px))+(px*px*px)+((px*x166)))),IKFAST_ATAN2_MAGTHRESH); +if(!x170.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x169.value)))+(x170.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[4]; +IkReal x171=IKcos(j0); +IkReal x172=IKsin(j0); +IkReal x173=((2.0)*sj1); +IkReal x174=(px*x171); +IkReal x175=(py*x172); +evalcond[0]=((((-1.0)*py*x171))+((px*x172))); +evalcond[1]=((-0.1)+((sj1*x174))+((sj1*x175))); +evalcond[2]=((((0.1)*sj1))+(((-1.0)*x175))+(((-1.0)*x174))); +evalcond[3]=((-0.19)+(((-1.0)*(px*px)))+((x173*x174))+((x173*x175))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +CheckValue x176=IKPowWithIntegerCheck(IKsign(((((10.0)*(px*px)))+(((10.0)*(py*py))))),-1); +if(!x176.valid){ +continue; +} +CheckValue x177 = IKatan2WithCheck(IkReal((py*sj1)),IkReal((px*sj1)),IKFAST_ATAN2_MAGTHRESH); +if(!x177.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x176.value)))+(x177.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[4]; +IkReal x178=IKcos(j0); +IkReal x179=IKsin(j0); +IkReal x180=((2.0)*sj1); +IkReal x181=(px*x178); +IkReal x182=(py*x179); +evalcond[0]=((((-1.0)*py*x178))+((px*x179))); +evalcond[1]=((-0.1)+((sj1*x182))+((sj1*x181))); +evalcond[2]=((((0.1)*sj1))+(((-1.0)*x181))+(((-1.0)*x182))); +evalcond[3]=((-0.19)+(((-1.0)*(px*px)))+((x180*x181))+((x180*x182))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x183=((10.0)*sj1); +CheckValue x184 = IKatan2WithCheck(IkReal(py),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x184.valid){ +continue; +} +CheckValue x185=IKPowWithIntegerCheck(IKsign((((x183*(py*py)))+((x183*(px*px))))),-1); +if(!x185.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x184.value)+(((1.5707963267949)*(x185.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[4]; +IkReal x186=IKcos(j0); +IkReal x187=IKsin(j0); +IkReal x188=((2.0)*sj1); +IkReal x189=(px*x186); +IkReal x190=(py*x187); +evalcond[0]=((((-1.0)*py*x186))+((px*x187))); +evalcond[1]=((-0.1)+((sj1*x190))+((sj1*x189))); +evalcond[2]=((((-1.0)*x190))+(((0.1)*sj1))+(((-1.0)*x189))); +evalcond[3]=((-0.19)+(((-1.0)*(px*px)))+((x188*x190))+((x188*x189))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j0] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x191=(py*sj1); +IkReal x192=((0.9)*cj2); +IkReal x193=(px*sj1); +IkReal x194=((0.9)*cj1*sj2); +CheckValue x195 = IKatan2WithCheck(IkReal((x191+((x191*x192))+((py*x194)))),IkReal((x193+((x192*x193))+((px*x194)))),IKFAST_ATAN2_MAGTHRESH); +if(!x195.valid){ +continue; +} +CheckValue x196=IKPowWithIntegerCheck(IKsign(((px*px)+(py*py))),-1); +if(!x196.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x195.value)+(((1.5707963267949)*(x196.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[5]; +IkReal x197=IKcos(j0); +IkReal x198=IKsin(j0); +IkReal x199=(cj1*pz); +IkReal x200=((0.9)*cj2); +IkReal x201=((2.0)*sj1); +IkReal x202=((0.9)*sj2); +IkReal x203=((1.0)*cj1); +IkReal x204=(px*x197); +IkReal x205=(py*x198); +evalcond[0]=((((-1.0)*py*x197))+((px*x198))); +evalcond[1]=((-1.0)+((sj1*x204))+((sj1*x205))+x199+(((-1.0)*x200))); +evalcond[2]=((((-1.0)*x203*x204))+(((-1.0)*x203*x205))+x202+((pz*sj1))); +evalcond[3]=(sj1+((cj1*x202))+((sj1*x200))+(((-1.0)*x204))+(((-1.0)*x205))); +evalcond[4]=((-0.19)+(((-1.0)*(px*px)))+((x201*x204))+((x201*x205))+(((-1.0)*(pz*pz)))+(((2.0)*x199))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x206=((0.9)*cj2); +IkReal x207=((1.0)*cj1*pz); +CheckValue x208 = IKatan2WithCheck(IkReal(((((-1.0)*py*x207))+((py*x206))+py)),IkReal(((((-1.0)*px*x207))+((px*x206))+px)),IKFAST_ATAN2_MAGTHRESH); +if(!x208.valid){ +continue; +} +CheckValue x209=IKPowWithIntegerCheck(IKsign((((sj1*(py*py)))+((sj1*(px*px))))),-1); +if(!x209.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(x208.value)+(((1.5707963267949)*(x209.value)))); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[5]; +IkReal x210=IKcos(j0); +IkReal x211=IKsin(j0); +IkReal x212=(cj1*pz); +IkReal x213=((0.9)*cj2); +IkReal x214=((2.0)*sj1); +IkReal x215=((0.9)*sj2); +IkReal x216=((1.0)*cj1); +IkReal x217=(px*x210); +IkReal x218=(py*x211); +evalcond[0]=((((-1.0)*py*x210))+((px*x211))); +evalcond[1]=((-1.0)+x212+((sj1*x218))+((sj1*x217))+(((-1.0)*x213))); +evalcond[2]=(x215+((pz*sj1))+(((-1.0)*x216*x217))+(((-1.0)*x216*x218))); +evalcond[3]=(sj1+(((-1.0)*x217))+(((-1.0)*x218))+((sj1*x213))+((cj1*x215))); +evalcond[4]=((-0.19)+(((-1.0)*(px*px)))+((x214*x218))+((x214*x217))+(((2.0)*x212))+(((-1.0)*(pz*pz)))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[1], cj0array[1], sj0array[1]; +bool j0valid[1]={false}; +_nj0 = 1; +IkReal x219=(pz*sj1); +IkReal x220=((0.9)*sj2); +CheckValue x221=IKPowWithIntegerCheck(IKsign((((cj1*(px*px)))+((cj1*(py*py))))),-1); +if(!x221.valid){ +continue; +} +CheckValue x222 = IKatan2WithCheck(IkReal((((py*x219))+((py*x220)))),IkReal((((px*x220))+((px*x219)))),IKFAST_ATAN2_MAGTHRESH); +if(!x222.valid){ +continue; +} +j0array[0]=((-1.5707963267949)+(((1.5707963267949)*(x221.value)))+(x222.value)); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +for(int ij0 = 0; ij0 < 1; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 1; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; +{ +IkReal evalcond[5]; +IkReal x223=IKcos(j0); +IkReal x224=IKsin(j0); +IkReal x225=(cj1*pz); +IkReal x226=((0.9)*cj2); +IkReal x227=((2.0)*sj1); +IkReal x228=((0.9)*sj2); +IkReal x229=((1.0)*cj1); +IkReal x230=(px*x223); +IkReal x231=(py*x224); +evalcond[0]=(((px*x224))+(((-1.0)*py*x223))); +evalcond[1]=((-1.0)+((sj1*x231))+((sj1*x230))+x225+(((-1.0)*x226))); +evalcond[2]=(x228+((pz*sj1))+(((-1.0)*x229*x230))+(((-1.0)*x229*x231))); +evalcond[3]=(sj1+((cj1*x228))+((sj1*x226))+(((-1.0)*x231))+(((-1.0)*x230))); +evalcond[4]=((-0.19)+(((-1.0)*(px*px)))+(((-1.0)*(pz*pz)))+((x227*x231))+((x227*x230))+(((-1.0)*(py*py)))+(((2.0)*x225))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x233 = IKatan2WithCheck(IkReal(((-1.0)*py)),IkReal(px),IKFAST_ATAN2_MAGTHRESH); +if(!x233.valid){ +continue; +} +IkReal x232=x233.value; +j0array[0]=((-1.0)*x232); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x232))); +sj0array[1]=IKsin(j0array[1]); +cj0array[1]=IKcos(j0array[1]); +if( j0array[0] > IKPI ) +{ + j0array[0]-=IK2PI; +} +else if( j0array[0] < -IKPI ) +{ j0array[0]+=IK2PI; +} +j0valid[0] = true; +if( j0array[1] > IKPI ) +{ + j0array[1]-=IK2PI; +} +else if( j0array[1] < -IKPI ) +{ j0array[1]+=IK2PI; +} +j0valid[1] = true; +for(int ij0 = 0; ij0 < 2; ++ij0) +{ +if( !j0valid[ij0] ) +{ + continue; +} +_ij0[0] = ij0; _ij0[1] = -1; +for(int iij0 = ij0+1; iij0 < 2; ++iij0) +{ +if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) +{ + j0valid[iij0]=false; _ij0[1] = iij0; break; +} +} +j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; + +{ +IkReal j1eval[3]; +IkReal x234=(py*sj0); +IkReal x235=(cj0*px); +IkReal x236=((90.0)*sj2); +IkReal x237=((90.0)*cj2); +j1eval[0]=((-1.00555555555556)+(((-1.0)*cj2))); +j1eval[1]=IKsign(((-181.0)+(((-180.0)*cj2)))); +j1eval[2]=((IKabs(((((-100.0)*x234))+(((-100.0)*x235))+(((-1.0)*x235*x237))+((pz*x236))+(((-1.0)*x234*x237)))))+(IKabs(((((-100.0)*pz))+(((-1.0)*x235*x236))+(((-1.0)*pz*x237))+(((-1.0)*x234*x236)))))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j1eval[3]; +IkReal x238=(cj0*px); +IkReal x239=((1.0)*cj2); +IkReal x240=(py*sj0); +IkReal x241=((9.0)*cj2); +IkReal x242=((10.0)*pz); +IkReal x243=(pz*sj2); +j1eval[0]=((((-1.0)*x243))+(((-1.0)*x238*x239))+(((-1.11111111111111)*x238))+(((-1.11111111111111)*x240))+(((-1.0)*x239*x240))); +j1eval[1]=IKsign(((((-9.0)*x243))+(((-10.0)*x238))+(((-1.0)*x238*x241))+(((-10.0)*x240))+(((-1.0)*x240*x241)))); +j1eval[2]=((IKabs(((((-8.1)*cj2*sj2))+(((-1.0)*x238*x242))+(((-9.0)*sj2))+(((-1.0)*x240*x242)))))+(IKabs(((-10.0)+(((-18.0)*cj2))+((pz*x242))+(((-8.1)*(cj2*cj2))))))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j1eval[3]; +IkReal x244=py*py; +IkReal x245=cj0*cj0; +IkReal x246=px*px; +IkReal x247=pz*pz; +IkReal x248=((9.0)*cj2); +IkReal x249=(py*sj0); +IkReal x250=((9.0)*sj2); +IkReal x251=(cj0*px); +IkReal x252=(x244*x245); +IkReal x253=(x245*x246); +j1eval[0]=((((-2.0)*x249*x251))+(((-1.0)*x247))+(((-1.0)*x244))+x252+(((-1.0)*x253))); +j1eval[1]=IKsign(((((-10.0)*x253))+(((10.0)*x252))+(((-10.0)*x247))+(((-10.0)*x244))+(((-20.0)*x249*x251)))); +j1eval[2]=((IKabs(((((-1.0)*pz*x248))+(((-1.0)*x250*x251))+(((-10.0)*pz))+(((-1.0)*x249*x250)))))+(IKabs(((((-1.0)*x248*x249))+(((-10.0)*x251))+((pz*x250))+(((-1.0)*x248*x251))+(((-10.0)*x249)))))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 ) +{ +continue; // no branches [j1] + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +IkReal x254=cj0*cj0; +IkReal x255=py*py; +IkReal x256=((9.0)*cj2); +IkReal x257=(py*sj0); +IkReal x258=((9.0)*sj2); +IkReal x259=(cj0*px); +IkReal x260=((10.0)*x255); +CheckValue x261 = IKatan2WithCheck(IkReal(((((-10.0)*x257))+(((-10.0)*x259))+((pz*x258))+(((-1.0)*x256*x257))+(((-1.0)*x256*x259)))),IkReal(((((-1.0)*x257*x258))+(((-10.0)*pz))+(((-1.0)*pz*x256))+(((-1.0)*x258*x259)))),IKFAST_ATAN2_MAGTHRESH); +if(!x261.valid){ +continue; +} +CheckValue x262=IKPowWithIntegerCheck(IKsign(((((-20.0)*x257*x259))+((x254*x260))+(((-10.0)*x254*(px*px)))+(((-1.0)*x260))+(((-10.0)*(pz*pz))))),-1); +if(!x262.valid){ +continue; +} +j1array[0]=((-1.5707963267949)+(x261.value)+(((1.5707963267949)*(x262.value)))); +sj1array[0]=IKsin(j1array[0]); +cj1array[0]=IKcos(j1array[0]); +if( j1array[0] > IKPI ) +{ + j1array[0]-=IK2PI; +} +else if( j1array[0] < -IKPI ) +{ j1array[0]+=IK2PI; +} +j1valid[0] = true; +for(int ij1 = 0; ij1 < 1; ++ij1) +{ +if( !j1valid[ij1] ) +{ + continue; +} +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < 1; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; +{ +IkReal evalcond[5]; +IkReal x263=IKcos(j1); +IkReal x264=IKsin(j1); +IkReal x265=(py*sj0); +IkReal x266=((0.9)*sj2); +IkReal x267=((0.9)*cj2); +IkReal x268=(cj0*px); +IkReal x269=((1.0)*x263); +IkReal x270=(pz*x263); +IkReal x271=((2.0)*x264); +evalcond[0]=((((-1.0)*x263*x267))+((x264*x266))+pz+(((-1.0)*x269))); +evalcond[1]=((-1.0)+((x264*x268))+((x264*x265))+x270+(((-1.0)*x267))); +evalcond[2]=(x266+(((-1.0)*x268*x269))+((pz*x264))+(((-1.0)*x265*x269))); +evalcond[3]=((((-1.0)*x268))+(((-1.0)*x265))+((x264*x267))+((x263*x266))+x264); +evalcond[4]=((-0.19)+(((-1.0)*(px*px)))+(((2.0)*x270))+(((-1.0)*(pz*pz)))+((x265*x271))+(((-1.0)*(py*py)))+((x268*x271))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +IkReal x272=(py*sj0); +IkReal x273=((9.0)*cj2); +IkReal x274=((9.0)*sj2); +IkReal x275=((10.0)*cj0*px); +CheckValue x276=IKPowWithIntegerCheck(IKsign(((((-1.0)*x272*x273))+(((-1.0)*pz*x274))+(((-1.0)*cj0*px*x273))+(((-10.0)*x272))+(((-1.0)*x275)))),-1); +if(!x276.valid){ +continue; +} +CheckValue x277 = IKatan2WithCheck(IkReal(((-10.0)+(((-18.0)*cj2))+(((10.0)*(pz*pz)))+(((-8.1)*(cj2*cj2))))),IkReal(((((-1.0)*pz*x275))+(((-8.1)*cj2*sj2))+(((-10.0)*pz*x272))+(((-1.0)*x274)))),IKFAST_ATAN2_MAGTHRESH); +if(!x277.valid){ +continue; +} +j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x276.value)))+(x277.value)); +sj1array[0]=IKsin(j1array[0]); +cj1array[0]=IKcos(j1array[0]); +if( j1array[0] > IKPI ) +{ + j1array[0]-=IK2PI; +} +else if( j1array[0] < -IKPI ) +{ j1array[0]+=IK2PI; +} +j1valid[0] = true; +for(int ij1 = 0; ij1 < 1; ++ij1) +{ +if( !j1valid[ij1] ) +{ + continue; +} +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < 1; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; +{ +IkReal evalcond[5]; +IkReal x278=IKcos(j1); +IkReal x279=IKsin(j1); +IkReal x280=(py*sj0); +IkReal x281=((0.9)*sj2); +IkReal x282=((0.9)*cj2); +IkReal x283=(cj0*px); +IkReal x284=((1.0)*x278); +IkReal x285=(pz*x278); +IkReal x286=((2.0)*x279); +evalcond[0]=(((x279*x281))+pz+(((-1.0)*x284))+(((-1.0)*x278*x282))); +evalcond[1]=((-1.0)+((x279*x280))+((x279*x283))+x285+(((-1.0)*x282))); +evalcond[2]=((((-1.0)*x280*x284))+x281+((pz*x279))+(((-1.0)*x283*x284))); +evalcond[3]=(((x279*x282))+((x278*x281))+x279+(((-1.0)*x280))+(((-1.0)*x283))); +evalcond[4]=((-0.19)+(((-1.0)*(px*px)))+(((-1.0)*(pz*pz)))+(((2.0)*x285))+((x280*x286))+(((-1.0)*(py*py)))+((x283*x286))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +IkReal x287=(py*sj0); +IkReal x288=(cj0*px); +IkReal x289=((90.0)*sj2); +IkReal x290=((90.0)*cj2); +CheckValue x291 = IKatan2WithCheck(IkReal(((((-100.0)*x288))+(((-100.0)*x287))+(((-1.0)*x287*x290))+(((-1.0)*x288*x290))+((pz*x289)))),IkReal(((((-100.0)*pz))+(((-1.0)*x287*x289))+(((-1.0)*x288*x289))+(((-1.0)*pz*x290)))),IKFAST_ATAN2_MAGTHRESH); +if(!x291.valid){ +continue; +} +CheckValue x292=IKPowWithIntegerCheck(IKsign(((-181.0)+(((-180.0)*cj2)))),-1); +if(!x292.valid){ +continue; +} +j1array[0]=((-1.5707963267949)+(x291.value)+(((1.5707963267949)*(x292.value)))); +sj1array[0]=IKsin(j1array[0]); +cj1array[0]=IKcos(j1array[0]); +if( j1array[0] > IKPI ) +{ + j1array[0]-=IK2PI; +} +else if( j1array[0] < -IKPI ) +{ j1array[0]+=IK2PI; +} +j1valid[0] = true; +for(int ij1 = 0; ij1 < 1; ++ij1) +{ +if( !j1valid[ij1] ) +{ + continue; +} +_ij1[0] = ij1; _ij1[1] = -1; +for(int iij1 = ij1+1; iij1 < 1; ++iij1) +{ +if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) +{ + j1valid[iij1]=false; _ij1[1] = iij1; break; +} +} +j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; +{ +IkReal evalcond[5]; +IkReal x293=IKcos(j1); +IkReal x294=IKsin(j1); +IkReal x295=(py*sj0); +IkReal x296=((0.9)*sj2); +IkReal x297=((0.9)*cj2); +IkReal x298=(cj0*px); +IkReal x299=((1.0)*x293); +IkReal x300=(pz*x293); +IkReal x301=((2.0)*x294); +evalcond[0]=((((-1.0)*x299))+(((-1.0)*x293*x297))+pz+((x294*x296))); +evalcond[1]=((-1.0)+(((-1.0)*x297))+x300+((x294*x298))+((x294*x295))); +evalcond[2]=(((pz*x294))+x296+(((-1.0)*x298*x299))+(((-1.0)*x295*x299))); +evalcond[3]=((((-1.0)*x295))+(((-1.0)*x298))+x294+((x294*x297))+((x293*x296))); +evalcond[4]=((-0.19)+(((-1.0)*(px*px)))+((x298*x301))+((x295*x301))+(((-1.0)*(pz*pz)))+(((2.0)*x300))+(((-1.0)*(py*py)))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(3); +vinfos[0].jointtype = 1; +vinfos[0].foffset = j0; +vinfos[0].indices[0] = _ij0[0]; +vinfos[0].indices[1] = _ij0[1]; +vinfos[0].maxsolutions = _nj0; +vinfos[1].jointtype = 1; +vinfos[1].foffset = j1; +vinfos[1].indices[0] = _ij1[0]; +vinfos[1].indices[1] = _ij1[1]; +vinfos[1].maxsolutions = _nj1; +vinfos[2].jointtype = 1; +vinfos[2].foffset = j2; +vinfos[2].indices[0] = _ij2[0]; +vinfos[2].indices[1] = _ij2[1]; +vinfos[2].maxsolutions = _nj2; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} +} +} +} +return solutions.GetNumSolutions()>0; +} +static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) { + IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2]; + if( det < 0 ) { + numroots=0; + } + else if( det == 0 ) { + rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0]; + numroots = 1; + } + else { + det = IKsqrt(det); + rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]); + rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]); + numroots = 2; + } +} +}; + + +/// solves the inverse kinematics equations. +/// \param pfree is an array specifying the free joints of the chain. +IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions) { +IKSolver solver; +return solver.ComputeIk(eetrans,eerot,pfree,solutions); +} + +IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions, void* pOpenRAVEManip) { +IKSolver solver; +return solver.ComputeIk(eetrans,eerot,pfree,solutions); +} + +IKFAST_API const char* GetKinematicsHash() { return "2100e3b7e48f1b3ecd771744f501d244"; } + +IKFAST_API const char* GetIkFastVersion() { return "0x10000049"; } + +#ifdef IKFAST_NAMESPACE +} // end namespace +#endif + +#ifndef IKFAST_NO_MAIN +#include +#include +#ifdef IKFAST_NAMESPACE +using namespace IKFAST_NAMESPACE; +#endif +int main(int argc, char** argv) +{ + if( argc != 12+GetNumFreeParameters()+1 ) { + printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" + "Returns the ik solutions given the transformation of the end effector specified by\n" + "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" + "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters()); + return 1; + } + + IkSolutionList solutions; + std::vector vfree(GetNumFreeParameters()); + IkReal eerot[9],eetrans[3]; + eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]); + eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]); + eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]); + for(std::size_t i = 0; i < vfree.size(); ++i) + vfree[i] = atof(argv[13+i]); + bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + + if( !bSuccess ) { + fprintf(stderr,"Failed to get ik solution\n"); + return -1; + } + + printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions()); + std::vector solvalues(GetNumJoints()); + for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) { + const IkSolutionBase& sol = solutions.GetSolution(i); + printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); + for( std::size_t j = 0; j < solvalues.size(); ++j) + printf("%.15f, ", solvalues[j]); + printf("\n"); + } + return 0; +} + +#endif diff --git a/project_perry_project_perry_ikfast_plugin/update_ikfast_plugin.sh b/project_perry_project_perry_ikfast_plugin/update_ikfast_plugin.sh new file mode 100644 index 0000000..586ef71 --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/update_ikfast_plugin.sh @@ -0,0 +1,22 @@ +search_mode=OPTIMIZE_MAX_JOINT +srdf_filename=project_perry.srdf +robot_name_in_srdf=project_perry +moveit_config_pkg=project_perry_moveit_config +robot_name=project_perry +planning_group_name=project_perry +ikfast_plugin_pkg=project_perry_project_perry_ikfast_plugin +base_link_name=base_link +eef_link_name=gripper_link +ikfast_output_path=/ws2/ik/project_perry_project_perry_ikfast_plugin/src/project_perry_project_perry_ikfast_solver.cpp + +rosrun moveit_kinematics create_ikfast_moveit_plugin.py\ + --search_mode=$search_mode\ + --srdf_filename=$srdf_filename\ + --robot_name_in_srdf=$robot_name_in_srdf\ + --moveit_config_pkg=$moveit_config_pkg\ + $robot_name\ + $planning_group_name\ + $ikfast_plugin_pkg\ + $base_link_name\ + $eef_link_name\ + $ikfast_output_path From 94c0a16c80512b492ad7d4e300df0eebc32a7c51 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:44:42 -0500 Subject: [PATCH 09/24] Set up CMake for IKFast plugin to parse version number like other projects --- .../CMakeLists.txt | 12 ++++++++++ .../package.in.xml | 23 +++++++++++++++++++ 2 files changed, 35 insertions(+) create mode 100644 project_perry_project_perry_ikfast_plugin/package.in.xml diff --git a/project_perry_project_perry_ikfast_plugin/CMakeLists.txt b/project_perry_project_perry_ikfast_plugin/CMakeLists.txt index d040cbb..2dbe67a 100644 --- a/project_perry_project_perry_ikfast_plugin/CMakeLists.txt +++ b/project_perry_project_perry_ikfast_plugin/CMakeLists.txt @@ -45,4 +45,16 @@ ament_export_dependencies(pluginlib) ament_export_dependencies(rclcpp) ament_export_dependencies(tf2_kdl) ament_export_dependencies(tf2_eigen) + +# Parse version number from the file +file(READ "../version" version_input) +string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) + +# Fill version number into package.xml +configure_file( + package.in.xml + ${CMAKE_CURRENT_SOURCE_DIR}/package.xml + @ONLY +) + ament_package() diff --git a/project_perry_project_perry_ikfast_plugin/package.in.xml b/project_perry_project_perry_ikfast_plugin/package.in.xml new file mode 100644 index 0000000..5008075 --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/package.in.xml @@ -0,0 +1,23 @@ + + + project_perry_project_perry_ikfast_plugin + 0.0.0 + IKFast plugin for project_perry + BSD + noah + ament_cmake + + ament_cmake + + + moveit_core + pluginlib + tf2_eigen + tf2_kdl + liblapack-dev + rclcpp + moveit_core + pluginlib + liblapack-dev + rclcpp + From 2c3f8587d10188512c7198b3cc60d1f63d5e0ddd Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:50:42 -0500 Subject: [PATCH 10/24] Experimenting with running build job on a subdirectory --- .github/workflows/ros-build.yaml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index a7e70ca..552c6b4 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -1,4 +1,4 @@ -name: Build package +name: Build packages on: push: @@ -6,11 +6,15 @@ on: - main jobs: - build: + build-description: permissions: contents: write packages: read + defaults: + run: + working-directory: ./umrt-project-perry-description + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main with: build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 From 43035fb2af5d8d3512cc643d02054c009e72fa7f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:51:39 -0500 Subject: [PATCH 11/24] Switched build workflow to be hit by initial_configs branch for testing --- .github/workflows/ros-build.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index 552c6b4..c7510e4 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -3,7 +3,8 @@ name: Build packages on: push: branches: - - main + #- main TODO: Switch back + - initial_configs jobs: build-description: From 969632435f3835570b72fc7c7bc16033e6fabe08 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:52:18 -0500 Subject: [PATCH 12/24] Fixed branch name --- .github/workflows/ros-build.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index c7510e4..0ffc220 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -4,7 +4,7 @@ on: push: branches: #- main TODO: Switch back - - initial_configs + - initial-configs jobs: build-description: From 3303f2b163f5733de2a8f2ed1eb27fa37051f86f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Tue, 1 Jul 2025 23:53:39 -0500 Subject: [PATCH 13/24] Experimenting with workflow --- .github/workflows/ros-build.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index 0ffc220..17a95f9 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -12,9 +12,6 @@ jobs: contents: write packages: read - defaults: - run: - working-directory: ./umrt-project-perry-description uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main with: From 7b26071680834f767d3d1867026fa832cbdee52f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Wed, 2 Jul 2025 00:04:27 -0500 Subject: [PATCH 14/24] Experimenting with new version of ros-build workflow --- .github/workflows/ros-build.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index 17a95f9..744cac3 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -12,12 +12,12 @@ jobs: contents: write packages: read - - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@ros-working-directory with: build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest publish_repo: UMRoboticsTeam/umrt-apt-repo + working_directory: umrt-project-perry-description secrets: APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} From 594639a7b4604e3ae5fcf87bc94c2bcedf58661f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Wed, 2 Jul 2025 00:05:47 -0500 Subject: [PATCH 15/24] Switched to using symlinks to version file for packages in order to avoid messing with ros-build workflow to let us inject a subdirectory into the version parsing process --- README.md | 2 ++ project_perry_project_perry_ikfast_plugin/CMakeLists.txt | 2 +- project_perry_project_perry_ikfast_plugin/version | 1 + umrt-project-perry-description/CMakeLists.txt | 2 +- umrt-project-perry-description/version | 1 + umrt-project-perry-moveit-config/CMakeLists.txt | 2 +- umrt-project-perry-moveit-config/version | 1 + 7 files changed, 8 insertions(+), 3 deletions(-) create mode 120000 project_perry_project_perry_ikfast_plugin/version create mode 120000 umrt-project-perry-description/version create mode 120000 umrt-project-perry-moveit-config/version diff --git a/README.md b/README.md index 7aa0411..6cc3eda 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,8 @@ For example, if a URDF is changed, that would affect the ros2_control configurat If each of these lived in separate repos, that is 4 PRs to create, 4 new versions which have to be deployed, and if one doesn't get updated somewhere it will cause major problems. Thus, by organising these into a monorepo this is condensed to 1 PR, and 1 version to deploy/manage. +One unusual technique used in this project, which is used to avoid complicating the ros-build workflow with additional parameters, is to symlink the version file into each of the packages. + # UMRT Project Perry Description This is the URDF and RViz files for the Project Perry robotics arm model. diff --git a/project_perry_project_perry_ikfast_plugin/CMakeLists.txt b/project_perry_project_perry_ikfast_plugin/CMakeLists.txt index 2dbe67a..b462d03 100644 --- a/project_perry_project_perry_ikfast_plugin/CMakeLists.txt +++ b/project_perry_project_perry_ikfast_plugin/CMakeLists.txt @@ -47,7 +47,7 @@ ament_export_dependencies(tf2_kdl) ament_export_dependencies(tf2_eigen) # Parse version number from the file -file(READ "../version" version_input) +file(READ "version" version_input) string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) # Fill version number into package.xml diff --git a/project_perry_project_perry_ikfast_plugin/version b/project_perry_project_perry_ikfast_plugin/version new file mode 120000 index 0000000..1748d5b --- /dev/null +++ b/project_perry_project_perry_ikfast_plugin/version @@ -0,0 +1 @@ +../version \ No newline at end of file diff --git a/umrt-project-perry-description/CMakeLists.txt b/umrt-project-perry-description/CMakeLists.txt index f42f867..37205e2 100644 --- a/umrt-project-perry-description/CMakeLists.txt +++ b/umrt-project-perry-description/CMakeLists.txt @@ -21,7 +21,7 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() # Parse version number from the file -file(READ "../version" version_input) +file(READ "version" version_input) string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) # Fill version number into package.xml diff --git a/umrt-project-perry-description/version b/umrt-project-perry-description/version new file mode 120000 index 0000000..1748d5b --- /dev/null +++ b/umrt-project-perry-description/version @@ -0,0 +1 @@ +../version \ No newline at end of file diff --git a/umrt-project-perry-moveit-config/CMakeLists.txt b/umrt-project-perry-moveit-config/CMakeLists.txt index 722f6c6..cddef2d 100644 --- a/umrt-project-perry-moveit-config/CMakeLists.txt +++ b/umrt-project-perry-moveit-config/CMakeLists.txt @@ -4,7 +4,7 @@ project(umrt-project-perry-moveit-config) find_package(ament_cmake REQUIRED) # Parse version number from the file -file(READ "../version" version_input) +file(READ "version" version_input) string(REGEX MATCH "([0-9]+)\.([0-9]+)\.([0-9]+)" _ ${version_input}) # Fill version number into package.xml diff --git a/umrt-project-perry-moveit-config/version b/umrt-project-perry-moveit-config/version new file mode 120000 index 0000000..1748d5b --- /dev/null +++ b/umrt-project-perry-moveit-config/version @@ -0,0 +1 @@ +../version \ No newline at end of file From dcf7f2ff36984802eee8c957d8520be44236e292 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Wed, 2 Jul 2025 01:13:20 -0500 Subject: [PATCH 16/24] Switched back to only running workflow for main branch now that this works --- .github/workflows/ros-build.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index 744cac3..1549305 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -3,8 +3,7 @@ name: Build packages on: push: branches: - #- main TODO: Switch back - - initial-configs + - main jobs: build-description: From 0144755bfdafa665f4f33a667f0752f1e5122232 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Thu, 3 Jul 2025 00:06:42 -0500 Subject: [PATCH 17/24] Added build jobs for all packages --- .github/workflows/ros-build.yaml | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index 1549305..bfee226 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -21,3 +21,35 @@ jobs: APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} + + build-moveit-config: + permissions: + contents: write + packages: read + + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@ros-working-directory + with: + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 + publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest + publish_repo: UMRoboticsTeam/umrt-apt-repo + working_directory: umrt-project-perry-moveit-config + secrets: + APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} + APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} + APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} + + build-ikfast-plugin: + permissions: + contents: write + packages: read + + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@ros-working-directory + with: + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 + publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest + publish_repo: UMRoboticsTeam/umrt-apt-repo + working_directory: project_perry_project_perry_ikfast_plugin + secrets: + APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} + APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} + APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} From 27954517ae12ea82f956feee5e4eaadcaa89fe7f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sun, 6 Jul 2025 22:33:22 -0500 Subject: [PATCH 18/24] Moved top-level URDF xacro here from umrt-arm-ros-firmware --- .../urdf/project_perry.urdf.xacro | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 umrt-project-perry-description/urdf/project_perry.urdf.xacro diff --git a/umrt-project-perry-description/urdf/project_perry.urdf.xacro b/umrt-project-perry-description/urdf/project_perry.urdf.xacro new file mode 100644 index 0000000..87ee10b --- /dev/null +++ b/umrt-project-perry-description/urdf/project_perry.urdf.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + From eebfb2d164f648678510b1ba4c65dd2ae8666473 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sun, 6 Jul 2025 22:38:08 -0500 Subject: [PATCH 19/24] Set up URDF xacro for package split --- .../urdf/project_perry.urdf.xacro | 6 +++--- umrt-project-perry-moveit-config/.setup_assistant | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/umrt-project-perry-description/urdf/project_perry.urdf.xacro b/umrt-project-perry-description/urdf/project_perry.urdf.xacro index 87ee10b..8d9a005 100644 --- a/umrt-project-perry-description/urdf/project_perry.urdf.xacro +++ b/umrt-project-perry-description/urdf/project_perry.urdf.xacro @@ -4,13 +4,13 @@ - + - + - + diff --git a/umrt-project-perry-moveit-config/.setup_assistant b/umrt-project-perry-moveit-config/.setup_assistant index 1937839..be45fb0 100644 --- a/umrt-project-perry-moveit-config/.setup_assistant +++ b/umrt-project-perry-moveit-config/.setup_assistant @@ -1,6 +1,6 @@ moveit_setup_assistant_config: urdf: - package: umrt-arm-ros-firmware + package: umrt-project-perry-description relative_path: urdf/project_perry.urdf.xacro srdf: relative_path: config/project_perry.srdf From 06ba48e27670c00b31fe1ba39a4232f2745702ed Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sun, 6 Jul 2025 23:42:53 -0500 Subject: [PATCH 20/24] Solidified version for build image --- .github/workflows/ros-build.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml index bfee226..b54a383 100644 --- a/.github/workflows/ros-build.yaml +++ b/.github/workflows/ros-build.yaml @@ -11,9 +11,9 @@ jobs: contents: write packages: read - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@ros-working-directory + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main with: - build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest publish_repo: UMRoboticsTeam/umrt-apt-repo working_directory: umrt-project-perry-description @@ -27,9 +27,9 @@ jobs: contents: write packages: read - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@ros-working-directory + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main with: - build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest publish_repo: UMRoboticsTeam/umrt-apt-repo working_directory: umrt-project-perry-moveit-config @@ -43,9 +43,9 @@ jobs: contents: write packages: read - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@ros-working-directory + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main with: - build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.16 + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest publish_repo: UMRoboticsTeam/umrt-apt-repo working_directory: project_perry_project_perry_ikfast_plugin From 2036d646834e0080c3a74ccbd45846270c389b01 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sun, 6 Jul 2025 23:58:20 -0500 Subject: [PATCH 21/24] Version bump --- version | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/version b/version index bd52db8..8a9ecc2 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.0.0 \ No newline at end of file +0.0.1 \ No newline at end of file From 621af942c18bc55a61a4bb67126f5b5b5d154ba4 Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sat, 19 Jul 2025 15:36:36 -0500 Subject: [PATCH 22/24] Split build workflow into one workflow for each package to avoid artifact name collision --- .github/workflows/description-build.yaml | 23 +++++++++ .github/workflows/ikfast-plugin-build.yaml | 23 +++++++++ .github/workflows/moveit-config-build.yaml | 23 +++++++++ .github/workflows/ros-build.yaml | 55 ---------------------- 4 files changed, 69 insertions(+), 55 deletions(-) create mode 100644 .github/workflows/description-build.yaml create mode 100644 .github/workflows/ikfast-plugin-build.yaml create mode 100644 .github/workflows/moveit-config-build.yaml delete mode 100644 .github/workflows/ros-build.yaml diff --git a/.github/workflows/description-build.yaml b/.github/workflows/description-build.yaml new file mode 100644 index 0000000..a8f7000 --- /dev/null +++ b/.github/workflows/description-build.yaml @@ -0,0 +1,23 @@ +name: Build umrt-project-perry-description package + +on: + push: + branches: + - main + +jobs: + build-description: + permissions: + contents: write + packages: read + + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main + with: + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 + publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest + publish_repo: UMRoboticsTeam/umrt-apt-repo + working_directory: umrt-project-perry-description + secrets: + APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} + APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} + APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} diff --git a/.github/workflows/ikfast-plugin-build.yaml b/.github/workflows/ikfast-plugin-build.yaml new file mode 100644 index 0000000..eb24406 --- /dev/null +++ b/.github/workflows/ikfast-plugin-build.yaml @@ -0,0 +1,23 @@ +name: Build project_perry_project_perry_ikfast_plugin package + +on: + push: + branches: + - main + +jobs: + build-ikfast-plugin: + permissions: + contents: write + packages: read + + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main + with: + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 + publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest + publish_repo: UMRoboticsTeam/umrt-apt-repo + working_directory: project_perry_project_perry_ikfast_plugin + secrets: + APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} + APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} + APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} diff --git a/.github/workflows/moveit-config-build.yaml b/.github/workflows/moveit-config-build.yaml new file mode 100644 index 0000000..e5e9844 --- /dev/null +++ b/.github/workflows/moveit-config-build.yaml @@ -0,0 +1,23 @@ +name: Build umrt-project-perry-moveit-config package + +on: + push: + branches: + - main + +jobs: + build-moveit-config: + permissions: + contents: write + packages: read + + uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main + with: + build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 + publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest + publish_repo: UMRoboticsTeam/umrt-apt-repo + working_directory: umrt-project-perry-moveit-config + secrets: + APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} + APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} + APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} diff --git a/.github/workflows/ros-build.yaml b/.github/workflows/ros-build.yaml deleted file mode 100644 index b54a383..0000000 --- a/.github/workflows/ros-build.yaml +++ /dev/null @@ -1,55 +0,0 @@ -name: Build packages - -on: - push: - branches: - - main - -jobs: - build-description: - permissions: - contents: write - packages: read - - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main - with: - build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 - publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest - publish_repo: UMRoboticsTeam/umrt-apt-repo - working_directory: umrt-project-perry-description - secrets: - APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} - APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} - APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} - - build-moveit-config: - permissions: - contents: write - packages: read - - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main - with: - build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 - publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest - publish_repo: UMRoboticsTeam/umrt-apt-repo - working_directory: umrt-project-perry-moveit-config - secrets: - APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} - APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} - APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} - - build-ikfast-plugin: - permissions: - contents: write - packages: read - - uses: UMRoboticsTeam/umrt-ci-cd/.github/workflows/ros-build.yaml@main - with: - build_image: ghcr.io/umroboticsteam/umrt-build:v0.0.26 - publish_image: ghcr.io/umroboticsteam/umrt-apt-image:latest - publish_repo: UMRoboticsTeam/umrt-apt-repo - working_directory: project_perry_project_perry_ikfast_plugin - secrets: - APT_SIGNING_PUBKEY: ${{ secrets.APT_SIGNING_PUBKEY }} - APT_SIGNING_KEY: ${{ secrets.APT_SIGNING_KEY }} - APT_DEPLOY_KEY: ${{ secrets.APT_DEPLOY_KEY }} From dd0d8e444d95123235ec150a239c5f2ce87e733b Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sat, 19 Jul 2025 15:39:16 -0500 Subject: [PATCH 23/24] Removed umrt-arm-ros-firmware exec dependency because then bloom requires to resolve it at build time for rosdep --- umrt-project-perry-moveit-config/package.in.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/umrt-project-perry-moveit-config/package.in.xml b/umrt-project-perry-moveit-config/package.in.xml index 9d3dbc5..3e30c9d 100644 --- a/umrt-project-perry-moveit-config/package.in.xml +++ b/umrt-project-perry-moveit-config/package.in.xml @@ -41,7 +41,7 @@ rviz_common rviz_default_plugins tf2_ros - umrt-arm-ros-firmware + warehouse_ros_mongo xacro From 53a800126fad2057e48d2d485d3ab59d0282a39f Mon Sep 17 00:00:00 2001 From: Noah Reeder Date: Sat, 19 Jul 2025 15:39:57 -0500 Subject: [PATCH 24/24] Version bump --- version | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/version b/version index 8a9ecc2..7bcd0e3 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.0.1 \ No newline at end of file +0.0.2 \ No newline at end of file