diff --git a/README.md b/README.md index 096a7cd..2d11490 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,7 @@ Refer to [docs/ConnectingRealRobot.md](docs/ConnectingRealRobot.md) * rs013n * rs020n * rs025n + * rs030n * rs80n ## Notes diff --git a/docs/ConnectingRealRobot-ja.md b/docs/ConnectingRealRobot-ja.md index 80ee0bd..a5f9926 100644 --- a/docs/ConnectingRealRobot-ja.md +++ b/docs/ConnectingRealRobot-ja.md @@ -14,6 +14,7 @@ |rs013n|||✓||✓|| |rs020n|✓||✓|||| |rs025n||||✓||| +|rs030n||||✓||| |rs080n||✓||✓||| ### 1.2 サポートしているASバージョン diff --git a/docs/ConnectingRealRobot.md b/docs/ConnectingRealRobot.md index c7eeb37..b6e9845 100644 --- a/docs/ConnectingRealRobot.md +++ b/docs/ConnectingRealRobot.md @@ -13,6 +13,7 @@ Controllers with ”✓” are available. |rs013n|||✓||✓|| |rs020n|✓||✓|||| |rs025n||||✓||| +|rs030n||||✓||| |rs080n||✓||✓||| ### 1.2 Supported AS version diff --git a/docs/README-ja.md b/docs/README-ja.md index 8c51e62..9c39c8a 100644 --- a/docs/README-ja.md +++ b/docs/README-ja.md @@ -54,6 +54,7 @@ roslaunch ***_moveit_config moveit_planning_execution.launch * rs013n * rs020n * rs025n + * rs030n * rs80n ## ノート diff --git a/khi_robot_bringup/config/rs030n_controllers.yaml b/khi_robot_bringup/config/rs030n_controllers.yaml new file mode 100644 index 0000000..a47bb1c --- /dev/null +++ b/khi_robot_bringup/config/rs030n_controllers.yaml @@ -0,0 +1,58 @@ + khi_robot_param: + robot: RS030N + arm: + arm1: + - rs030n_arm_controller + + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Position - Joint Position Trajectory Controller ------------------- + rs030n_arm_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + constraints: + goal_time: 2.0 # Defaults to zero + stopped_velocity_tolerance: 0.1 # Defaults to 0.01 + joint1: + trajectory: 0 + goal: 0.0 + joint2: + trajectory: 0 + goal: 0.0 + joint3: + trajectory: 0 + goal: 0.0 + joint4: + trajectory: 0 + goal: 0.0 + joint5: + trajectory: 0 + goal: 0.0 + joint6: + trajectory: 0 + goal: 0.0 + + state_publish_rate: 50 # Defaults to 50 + action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + + + # Joint Group Position Controller ------------------- + rs030n_joint_group_controller: + type: "position_controllers/JointGroupPositionController" + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 \ No newline at end of file diff --git a/khi_robot_bringup/launch/rs030n_bringup.launch b/khi_robot_bringup/launch/rs030n_bringup.launch new file mode 100644 index 0000000..3209a15 --- /dev/null +++ b/khi_robot_bringup/launch/rs030n_bringup.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_robot_test/CMakeLists.txt b/khi_robot_test/CMakeLists.txt index 6768eef..6c875f2 100644 --- a/khi_robot_test/CMakeLists.txt +++ b/khi_robot_test/CMakeLists.txt @@ -47,5 +47,6 @@ if (CATKIN_ENABLE_TESTING) add_rostest(tests/khi_robot_control/test_khi_robot_control_rs007l.xml) add_rostest(tests/khi_robot_control/test_khi_robot_control_rs013n.xml) add_rostest(tests/khi_robot_control/test_khi_robot_control_rs020n.xml) + add_rostest(tests/khi_robot_control/test_khi_robot_control_rs030n.xml) add_rostest(tests/khi_robot_control/test_khi_robot_control_rs080n.xml) endif() diff --git a/khi_robot_test/package.xml b/khi_robot_test/package.xml index 33a1fbe..f8c0d72 100644 --- a/khi_robot_test/package.xml +++ b/khi_robot_test/package.xml @@ -27,6 +27,7 @@ khi_rs007n_moveit_config khi_rs013n_moveit_config khi_rs020n_moveit_config + khi_rs030n_moveit_config khi_rs080n_moveit_config moveit_commander diff --git a/khi_robot_test/tests/khi_robot_control/test_khi_robot_control_rs030n.xml b/khi_robot_test/tests/khi_robot_control/test_khi_robot_control_rs030n.xml new file mode 100644 index 0000000..4a15420 --- /dev/null +++ b/khi_robot_test/tests/khi_robot_control/test_khi_robot_control_rs030n.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/.setup_assistant b/khi_rs030n_moveit_config/.setup_assistant new file mode 100644 index 0000000..38374ef --- /dev/null +++ b/khi_rs030n_moveit_config/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: khi_rs_description + relative_path: urdf/rs030n.urdf.xacro + xacro_args: "" + SRDF: + relative_path: config/khi_rs030n.srdf + CONFIG: + author_name: RS030N + author_email: kurita_taisuke@khi.co.jp + generated_timestamp: 1693526009 \ No newline at end of file diff --git a/khi_rs030n_moveit_config/CMakeLists.txt b/khi_rs030n_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..5e65bf8 --- /dev/null +++ b/khi_rs030n_moveit_config/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.1.3) +project(khi_rs030n_moveit_config) + +find_package(catkin REQUIRED) + +catkin_package() + +if (CATKIN_ENABLE_TESTING) + find_package(roslaunch REQUIRED) + roslaunch_add_file_check(tests/roslaunch_test_moveit_rs030n.xml) +endif() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/khi_rs030n_moveit_config/config/cartesian_limits.yaml b/khi_rs030n_moveit_config/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/khi_rs030n_moveit_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/khi_rs030n_moveit_config/config/chomp_planning.yaml b/khi_rs030n_moveit_config/config/chomp_planning.yaml new file mode 100644 index 0000000..eb9c912 --- /dev/null +++ b/khi_rs030n_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/khi_rs030n_moveit_config/config/controllers.yaml b/khi_rs030n_moveit_config/config/controllers.yaml new file mode 100644 index 0000000..eb99bff --- /dev/null +++ b/khi_rs030n_moveit_config/config/controllers.yaml @@ -0,0 +1,11 @@ +controller_list: + - name: rs030n_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/khi_rs030n_moveit_config/config/fake_controllers.yaml b/khi_rs030n_moveit_config/config/fake_controllers.yaml new file mode 100644 index 0000000..fa61c0d --- /dev/null +++ b/khi_rs030n_moveit_config/config/fake_controllers.yaml @@ -0,0 +1,17 @@ +controller_list: + - name: fake_manipulator_controller + type: $(arg fake_execution_type) + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - name: fake_tool_controller + type: $(arg fake_execution_type) + joints: + - joint6 +initial: # Define initial robot poses per group + - group: manipulator + pose: home \ No newline at end of file diff --git a/khi_rs030n_moveit_config/config/gazebo_controllers.yaml b/khi_rs030n_moveit_config/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/khi_rs030n_moveit_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/khi_rs030n_moveit_config/config/gazebo_khi_rs030n.urdf b/khi_rs030n_moveit_config/config/gazebo_khi_rs030n.urdf new file mode 100644 index 0000000..e8f21dc --- /dev/null +++ b/khi_rs030n_moveit_config/config/gazebo_khi_rs030n.urdf @@ -0,0 +1,305 @@ + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 1 + + + hardware_interface/PositionJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 1 + + + hardware_interface/PositionJointInterface + + + + + + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/config/khi_rs030n.srdf b/khi_rs030n_moveit_config/config/khi_rs030n.srdf new file mode 100644 index 0000000..2d22e52 --- /dev/null +++ b/khi_rs030n_moveit_config/config/khi_rs030n.srdf @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/config/kinematics.yaml b/khi_rs030n_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..a2c3e99 --- /dev/null +++ b/khi_rs030n_moveit_config/config/kinematics.yaml @@ -0,0 +1,7 @@ +manipulator: + kinematics_solver: khi_rs030n_manipulator_kinematics/IKFastKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.0001 + goal_position_tolerance: 0.0001 + goal_orientation_tolerance: 0.001 \ No newline at end of file diff --git a/khi_rs030n_moveit_config/config/ompl_planning.yaml b/khi_rs030n_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..0c37fe1 --- /dev/null +++ b/khi_rs030n_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,227 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + AITstar: + type: geometric::AITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1 + ABITstar: + type: geometric::ABITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000 + inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + BITstar: + type: geometric::BITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 +manipulator: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(joint1,joint2) + longest_valid_segment_fraction: 0.005 +tool: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar diff --git a/khi_rs030n_moveit_config/config/ros_controllers.yaml b/khi_rs030n_moveit_config/config/ros_controllers.yaml new file mode 100644 index 0000000..eb99bff --- /dev/null +++ b/khi_rs030n_moveit_config/config/ros_controllers.yaml @@ -0,0 +1,11 @@ +controller_list: + - name: rs030n_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/khi_rs030n_moveit_config/config/sensors_3d.yaml b/khi_rs030n_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..51010a3 --- /dev/null +++ b/khi_rs030n_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/khi_rs030n_moveit_config/config/simple_moveit_controllers.yaml b/khi_rs030n_moveit_config/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..4118c3b --- /dev/null +++ b/khi_rs030n_moveit_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,2 @@ +controller_list: + [] \ No newline at end of file diff --git a/khi_rs030n_moveit_config/config/stomp_planning.yaml b/khi_rs030n_moveit_config/config/stomp_planning.yaml new file mode 100644 index 0000000..25ef11d --- /dev/null +++ b/khi_rs030n_moveit_config/config/stomp_planning.yaml @@ -0,0 +1,78 @@ +stomp/manipulator: + group_name: manipulator + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/tool: + group_name: tool + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/khi_rs030n_moveit_config/launch/chomp_planning_pipeline.launch.xml b/khi_rs030n_moveit_config/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..ea20dcb --- /dev/null +++ b/khi_rs030n_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/default_warehouse_db.launch b/khi_rs030n_moveit_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..99d7016 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/demo.launch b/khi_rs030n_moveit_config/launch/demo.launch new file mode 100644 index 0000000..76bea61 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/demo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/demo_gazebo.launch b/khi_rs030n_moveit_config/launch/demo_gazebo.launch new file mode 100644 index 0000000..0ef8f95 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/khi_rs030n_moveit_config/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..2baed60 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/gazebo.launch b/khi_rs030n_moveit_config/launch/gazebo.launch new file mode 100644 index 0000000..e94e97b --- /dev/null +++ b/khi_rs030n_moveit_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/joystick_control.launch b/khi_rs030n_moveit_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/khi_rs030n_moveit_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/khi_rs030n_moveit_controller_manager.launch.xml b/khi_rs030n_moveit_config/launch/khi_rs030n_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..600428f --- /dev/null +++ b/khi_rs030n_moveit_config/launch/khi_rs030n_moveit_controller_manager.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/khi_rs030n_moveit_config/launch/khi_rs030n_moveit_sensor_manager.launch.xml b/khi_rs030n_moveit_config/launch/khi_rs030n_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/khi_rs030n_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/khi_rs030n_moveit_config/launch/move_group.launch b/khi_rs030n_moveit_config/launch/move_group.launch new file mode 100644 index 0000000..c20c478 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/move_group.launch @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/moveit.rviz b/khi_rs030n_moveit_config/launch/moveit.rviz new file mode 100644 index 0000000..6d87444 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/moveit.rviz @@ -0,0 +1,131 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 226 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + 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 + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Name: MotionPlanning + Planned Path: + Links: ~ + Loop Animation: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: ~ + Robot Alpha: 0.5 + Show Scene Robot: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.0 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.5 + Target Frame: world + Yaw: -0.6232355833053589 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 848 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1291 + X: 454 + Y: 25 diff --git a/khi_rs030n_moveit_config/launch/moveit_planning_execution.launch b/khi_rs030n_moveit_config/launch/moveit_planning_execution.launch new file mode 100644 index 0000000..a62ac06 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/moveit_planning_execution.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/moveit_rviz.launch b/khi_rs030n_moveit_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..1c8f0e4 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml b/khi_rs030n_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..a96cd10 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/ompl_planning_pipeline.launch.xml b/khi_rs030n_moveit_config/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..cc2fcc3 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/khi_rs030n_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/planning_context.launch b/khi_rs030n_moveit_config/launch/planning_context.launch new file mode 100644 index 0000000..be5eebc --- /dev/null +++ b/khi_rs030n_moveit_config/launch/planning_context.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/planning_pipeline.launch.xml b/khi_rs030n_moveit_config/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..4b4d0d6 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml b/khi_rs030n_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/khi_rs030n_moveit_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/khi_rs030n_moveit_config/launch/ros_controllers.launch b/khi_rs030n_moveit_config/launch/ros_controllers.launch new file mode 100644 index 0000000..c789c60 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/run_benchmark_ompl.launch b/khi_rs030n_moveit_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..c1d191c --- /dev/null +++ b/khi_rs030n_moveit_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/sensor_manager.launch.xml b/khi_rs030n_moveit_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..6aeddf8 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/setup_assistant.launch b/khi_rs030n_moveit_config/launch/setup_assistant.launch new file mode 100644 index 0000000..1a6f37c --- /dev/null +++ b/khi_rs030n_moveit_config/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/simple_moveit_controller_manager.launch.xml b/khi_rs030n_moveit_config/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..e88ce20 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/stomp_planning_pipeline.launch.xml b/khi_rs030n_moveit_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..f252ebc --- /dev/null +++ b/khi_rs030n_moveit_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/trajectory_execution.launch.xml b/khi_rs030n_moveit_config/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..20c3dfc --- /dev/null +++ b/khi_rs030n_moveit_config/launch/trajectory_execution.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/warehouse.launch b/khi_rs030n_moveit_config/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/launch/warehouse_settings.launch.xml b/khi_rs030n_moveit_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/khi_rs030n_moveit_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/khi_rs030n_moveit_config/package.xml b/khi_rs030n_moveit_config/package.xml new file mode 100644 index 0000000..12f4273 --- /dev/null +++ b/khi_rs030n_moveit_config/package.xml @@ -0,0 +1,41 @@ + + + khi_rs030n_moveit_config + 1.4.0 + + An automatically generated package with all the configuration and launch files for using the khi_rs030n with the MoveIt Motion Planning Framework + + RS030N + RS030N + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + khi_rs_description + + + diff --git a/khi_rs030n_moveit_config/tests/roslaunch_test_moveit_rs030n.xml b/khi_rs030n_moveit_config/tests/roslaunch_test_moveit_rs030n.xml new file mode 100644 index 0000000..db2b447 --- /dev/null +++ b/khi_rs030n_moveit_config/tests/roslaunch_test_moveit_rs030n.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/khi_rs_description/CMakeLists.txt b/khi_rs_description/CMakeLists.txt index 9a80d6c..cd648f8 100644 --- a/khi_rs_description/CMakeLists.txt +++ b/khi_rs_description/CMakeLists.txt @@ -12,6 +12,7 @@ if (CATKIN_ENABLE_TESTING) roslaunch_add_file_check(tests/roslaunch_test_rs013n.xml) roslaunch_add_file_check(tests/roslaunch_test_rs020n.xml) roslaunch_add_file_check(tests/roslaunch_test_rs025n.xml) + roslaunch_add_file_check(tests/roslaunch_test_rs030n.xml) roslaunch_add_file_check(tests/roslaunch_test_rs080n.xml) endif() diff --git a/khi_rs_description/config/rs030n_joint_limits.yaml b/khi_rs_description/config/rs030n_joint_limits.yaml new file mode 100644 index 0000000..fac4f6a --- /dev/null +++ b/khi_rs_description/config/rs030n_joint_limits.yaml @@ -0,0 +1,58 @@ +# 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: + joint1: + has_position_limits: true + min_position: -3.14159265359 + max_position: 3.14159265359 + has_velocity_limits: true + max_velocity: 3.078760800518 + has_acceleration_limits: true + max_acceleration: 4.68483115009004 + joint2: + has_position_limits: true + min_position: -1.83259571459 + max_position: 2.44346095279 + has_velocity_limits: true + max_velocity: 3.078760800518 + has_acceleration_limits: true + max_acceleration: 4.84328867428427 + joint3: + has_position_limits: true + min_position: -2.70526034059 + max_position: 2.35619449019 + has_velocity_limits: true + max_velocity: 3.16428193386572 + has_acceleration_limits: true + max_acceleration: 4.97469573133849 + joint4: + has_position_limits: true + min_position: -6.28318530718 + max_position: 6.28318530718 + has_velocity_limits: true + max_velocity: 4.44709893408155 + has_acceleration_limits: true + max_acceleration: 9.76690872804774 + joint5: + has_position_limits: true + min_position: -2.53072741539 + max_position: 2.53072741539 + has_velocity_limits: true + max_velocity: 4.44709893408155 + has_acceleration_limits: true + max_acceleration: 12.6051557088479 + joint6: + has_position_limits: true + min_position: -6.28318530718 + max_position: 6.28318530718 + has_velocity_limits: true + max_velocity: 6.15752160103599 + has_acceleration_limits: true + max_acceleration: 19.6602895095619 \ No newline at end of file diff --git a/khi_rs_description/meshes/RS030N_J0.stl b/khi_rs_description/meshes/RS030N_J0.stl new file mode 100644 index 0000000..1cd6bee Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J0.stl differ diff --git a/khi_rs_description/meshes/RS030N_J1.stl b/khi_rs_description/meshes/RS030N_J1.stl new file mode 100644 index 0000000..43a2c7a Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J1.stl differ diff --git a/khi_rs_description/meshes/RS030N_J2.stl b/khi_rs_description/meshes/RS030N_J2.stl new file mode 100644 index 0000000..0fd9d89 Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J2.stl differ diff --git a/khi_rs_description/meshes/RS030N_J3.stl b/khi_rs_description/meshes/RS030N_J3.stl new file mode 100644 index 0000000..c3760b1 Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J3.stl differ diff --git a/khi_rs_description/meshes/RS030N_J4.stl b/khi_rs_description/meshes/RS030N_J4.stl new file mode 100644 index 0000000..c2c884e Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J4.stl differ diff --git a/khi_rs_description/meshes/RS030N_J5.stl b/khi_rs_description/meshes/RS030N_J5.stl new file mode 100644 index 0000000..86ffdd0 Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J5.stl differ diff --git a/khi_rs_description/meshes/RS030N_J6.stl b/khi_rs_description/meshes/RS030N_J6.stl new file mode 100644 index 0000000..385b9c1 Binary files /dev/null and b/khi_rs_description/meshes/RS030N_J6.stl differ diff --git a/khi_rs_description/tests/roslaunch_test_rs030n.xml b/khi_rs_description/tests/roslaunch_test_rs030n.xml new file mode 100644 index 0000000..8e670e8 --- /dev/null +++ b/khi_rs_description/tests/roslaunch_test_rs030n.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/khi_rs_description/urdf/rs030n.urdf.xacro b/khi_rs_description/urdf/rs030n.urdf.xacro new file mode 100644 index 0000000..dfa8f88 --- /dev/null +++ b/khi_rs_description/urdf/rs030n.urdf.xacro @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/khi_rs_description/urdf/rs030n_macro.xacro b/khi_rs_description/urdf/rs030n_macro.xacro new file mode 100644 index 0000000..a2a84e1 --- /dev/null +++ b/khi_rs_description/urdf/rs030n_macro.xacro @@ -0,0 +1,259 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/khi_rs_gazebo/config/rs030n_gazebo_control.yaml b/khi_rs_gazebo/config/rs030n_gazebo_control.yaml new file mode 100644 index 0000000..7c8acb7 --- /dev/null +++ b/khi_rs_gazebo/config/rs030n_gazebo_control.yaml @@ -0,0 +1,51 @@ + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Position - Joint Position Trajectory Controller ------------------- + rs030n_arm_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + constraints: + goal_time: 2.0 # Defaults to zero + stopped_velocity_tolerance: 0.1 # Defaults to 0.01 + joint1: + trajectory: 0 + goal: 0.0 + joint2: + trajectory: 0 + goal: 0.0 + joint3: + trajectory: 0 + goal: 0.0 + joint4: + trajectory: 0 + goal: 0.0 + joint5: + trajectory: 0 + goal: 0.0 + joint6: + trajectory: 0 + goal: 0.0 + + state_publish_rate: 50 # Defaults to 50 + action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + + # Joint Group Position Controller ------------------- + rs030n_joint_group_controller: + type: "position_controllers/JointGroupPositionController" + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 \ No newline at end of file diff --git a/khi_rs_gazebo/launch/rs030n_gazebo_control.launch b/khi_rs_gazebo/launch/rs030n_gazebo_control.launch new file mode 100644 index 0000000..08c3f70 --- /dev/null +++ b/khi_rs_gazebo/launch/rs030n_gazebo_control.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + diff --git a/khi_rs_gazebo/launch/rs030n_world.launch b/khi_rs_gazebo/launch/rs030n_world.launch new file mode 100644 index 0000000..39b63f5 --- /dev/null +++ b/khi_rs_gazebo/launch/rs030n_world.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/khi_rs_ikfast_plugin/CMakeLists.txt b/khi_rs_ikfast_plugin/CMakeLists.txt index bbf6d28..9db7901 100644 --- a/khi_rs_ikfast_plugin/CMakeLists.txt +++ b/khi_rs_ikfast_plugin/CMakeLists.txt @@ -87,3 +87,18 @@ install( ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +set(IKFAST_LIBRARY_NAME khi_rs030n_manipulator_moveit_ikfast_plugin) + +find_package(LAPACK REQUIRED) + +add_library(${IKFAST_LIBRARY_NAME} src/khi_rs030n_manipulator_ikfast_moveit_plugin.cpp) +target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${LAPACK_LIBRARIES}) + +install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install( + FILES + khi_rs030n_manipulator_moveit_ikfast_plugin_description.xml + DESTINATION + ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/khi_rs_ikfast_plugin/khi_rs030n_manipulator_moveit_ikfast_plugin_description.xml b/khi_rs_ikfast_plugin/khi_rs030n_manipulator_moveit_ikfast_plugin_description.xml new file mode 100644 index 0000000..53b7647 --- /dev/null +++ b/khi_rs_ikfast_plugin/khi_rs030n_manipulator_moveit_ikfast_plugin_description.xml @@ -0,0 +1,6 @@ + + + + IKFast61 plugin for closed-form kinematics + + diff --git a/khi_rs_ikfast_plugin/package.xml b/khi_rs_ikfast_plugin/package.xml index 051219e..1a4d510 100644 --- a/khi_rs_ikfast_plugin/package.xml +++ b/khi_rs_ikfast_plugin/package.xml @@ -14,6 +14,7 @@ + moveit_core liblapack-dev diff --git a/khi_rs_ikfast_plugin/src/khi_rs030n_manipulator_ikfast_moveit_plugin.cpp b/khi_rs_ikfast_plugin/src/khi_rs030n_manipulator_ikfast_moveit_plugin.cpp new file mode 100644 index 0000000..d256c7e --- /dev/null +++ b/khi_rs_ikfast_plugin/src/khi_rs030n_manipulator_ikfast_moveit_plugin.cpp @@ -0,0 +1,1400 @@ +/********************************************************************* + * + * 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 Plugin Template for moveit + * + * AUTO-GENERATED by create_ikfast_moveit_plugin.py in arm_kinematics_tools + * You should run create_ikfast_moveit_plugin.py to generate your own plugin. + * + * Creates a kinematics plugin using the output of IKFast from OpenRAVE. + * This plugin and the move_group node can be used as a general + * kinematics service, from within the moveit planning environment, or in + * your own ROS node. + * + */ + +#include +#include +#include +#include + +// 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 ikfast_kinematics_plugin +{ +#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 "khi_rs030n_manipulator_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_; + bool active_; // Internal variable that indicates whether solvers are configured and ready + const std::string name_{ "ikfast" }; + + const std::vector& getJointNames() const + { + return joint_names_; + } + const std::vector& getLinkNames() const + { + return link_names_; + } + +public: + /** @class + * @brief Interface for an IKFast kinematics plugin + */ + IKFastKinematicsPlugin() : num_joints_(GetNumJoints()), active_(false) + { + srand(time(NULL)); + 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::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; + + /** + * @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; + + /** + * @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::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; + + /** + * @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::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; + + /** + * @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::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; + + /** + * @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::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::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; + + /** + * @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; + + /** + * @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); + +private: + bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_name, + const std::string& tip_name, double search_discretization); + + /** + * @brief Calls the IK solver from IKFast + * @return The number of solutions found + */ + int 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; + + double harmonize(const std::vector& ik_seed_state, std::vector& solution) const; + // void getOrderedSolutions(const std::vector &ik_seed_state, std::vector >& solslist); + void getClosestSolution(const IkSolutionList& solutions, const std::vector& ik_seed_state, + std::vector& solution) 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; + +}; // end class + +bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name, + const std::string& base_name, const std::string& tip_name, + double search_discretization) +{ + setValues(robot_description, group_name, base_name, tip_name, search_discretization); + + ros::NodeHandle node_handle("~/" + group_name); + + std::string robot; + lookupParam("robot", robot, std::string()); + + // IKFast56/61 + fillFreeParams(GetNumFreeParameters(), GetFreeParameters()); + + if (free_params_.size() > 1) + { + ROS_FATAL("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(DEFAULT_SEARCH_DISCRETIZATION); + } + + urdf::Model robot_model; + std::string xml_string; + + std::string urdf_xml, full_urdf_xml; + lookupParam("urdf_xml", urdf_xml, robot_description); + node_handle.searchParam(urdf_xml, full_urdf_xml); + + ROS_DEBUG_NAMED(name_, "Reading xml file from parameter server"); + if (!node_handle.getParam(full_urdf_xml, xml_string)) + { + ROS_FATAL_NAMED(name_, "Could not load the xml from parameter server: %s", urdf_xml.c_str()); + return false; + } + + robot_model.initString(xml_string); + + ROS_DEBUG_STREAM_NAMED(name_, "Reading joints and links from URDF"); + + urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame()); + while (link->name != base_frame_ && joint_names_.size() <= num_joints_) + { + ROS_DEBUG_NAMED(name_, "Link %s", link->name.c_str()); + link_names_.push_back(link->name); + urdf::JointSharedPtr joint = link->parent_joint; + if (joint) + { + if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) + { + ROS_DEBUG_STREAM_NAMED(name_, "Adding joint " << joint->name); + + joint_names_.push_back(joint->name); + float lower, upper; + int hasLimits; + if (joint->type != urdf::Joint::CONTINUOUS) + { + if (joint->safety) + { + lower = joint->safety->soft_lower_limit; + upper = joint->safety->soft_upper_limit; + } + else + { + lower = joint->limits->lower; + upper = joint->limits->upper; + } + hasLimits = 1; + } + else + { + lower = -M_PI; + upper = M_PI; + hasLimits = 0; + } + if (hasLimits) + { + joint_has_limits_vector_.push_back(true); + joint_min_vector_.push_back(lower); + joint_max_vector_.push_back(upper); + } + else + { + joint_has_limits_vector_.push_back(false); + joint_min_vector_.push_back(-M_PI); + joint_max_vector_.push_back(M_PI); + } + } + } + else + { + ROS_WARN_NAMED(name_, "no joint corresponding to %s", link->name.c_str()); + } + link = link->getParent(); + } + + if (joint_names_.size() != num_joints_) + { + ROS_FATAL_STREAM_NAMED(name_, "Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " + << 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 i = 0; i < num_joints_; ++i) + ROS_DEBUG_STREAM_NAMED(name_, joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " + << joint_has_limits_vector_[i]); + + active_ = true; + return true; +} + +void IKFastKinematicsPlugin::setSearchDiscretization(const std::map& discretization) +{ + if (discretization.empty()) + { + ROS_ERROR("The 'discretization' map is empty"); + return; + } + + if (redundant_joint_indices_.empty()) + { + ROS_ERROR_STREAM("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]]; + ROS_ERROR_STREAM("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) + { + ROS_ERROR_STREAM("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) +{ + ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver "); + return false; +} + +int 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] : NULL, 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] : NULL, solutions); + return solutions.GetNumSolutions(); + + case IKP_TranslationXAxisAngle4D: + case IKP_TranslationYAxisAngle4D: + case IKP_TranslationZAxisAngle4D: + // 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) + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_TranslationLocalGlobal6D: + // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end + // effector coordinate system. + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_Rotation3D: + case IKP_Lookat3D: + case IKP_TranslationXY2D: + case IKP_TranslationXYOrientation3D: + ROS_ERROR_NAMED(name_, "IK for this IkParameterizationType not implemented yet."); + return 0; + + 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] : NULL, solutions); + return solutions.GetNumSolutions(); + + 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] : NULL, solutions); + return solutions.GetNumSolutions(); + + 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] : NULL, solutions); + return solutions.GetNumSolutions(); + + default: + ROS_ERROR_NAMED(name_, "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] : NULL); + + // std::cout << "solution " << i << ":" ; + // for(int j=0;j& 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] : NULL); + + // 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]) + { + 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::harmonize(const std::vector& ik_seed_state, std::vector& solution) const +{ + double dist_sqr = 0; + std::vector ss = ik_seed_state; + for (size_t i = 0; i < ik_seed_state.size(); ++i) + { + while (ss[i] > 2 * M_PI) + { + ss[i] -= 2 * M_PI; + } + while (ss[i] < 2 * M_PI) + { + ss[i] += 2 * M_PI; + } + while (solution[i] > 2 * M_PI) + { + solution[i] -= 2 * M_PI; + } + while (solution[i] < 2 * M_PI) + { + solution[i] += 2 * M_PI; + } + dist_sqr += fabs(ik_seed_state[i] - solution[i]); + } + return dist_sqr; +} + +// void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector &ik_seed_state, +// std::vector >& solslist) +// { +// std::vector +// double mindist = 0; +// int minindex = -1; +// std::vector sol; +// for(size_t i=0;i& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + const IKCallbackFn solution_callback = 0; + 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::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + const IKCallbackFn solution_callback = 0; + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, + options); +} + +bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, + const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::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::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::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + ROS_DEBUG_STREAM_NAMED(name_, "searchPositionIK"); + + /// search_mode is currently fixed during code generation + SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT; + + // Check if there are no redundant joints + if (free_params_.size() == 0) + { + ROS_DEBUG_STREAM_NAMED(name_, "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 solution within joint limits + if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options)) + { + ROS_DEBUG_STREAM_NAMED(name_, "No solution whatsoever"); + error_code.val = moveit_msgs::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.empty()) + { + 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::MoveItErrorCodes::SUCCESS) + { + solution = solutions_obey_limits[i].value; + ROS_DEBUG_STREAM_NAMED(name_, "Solution passes callback"); + return true; + } + } + + ROS_DEBUG_STREAM_NAMED(name_, "Solution has error code " << error_code); + return false; + } + else + { + solution = solutions_obey_limits[0].value; + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return true; // no collision check callback provided + } + } + + // ------------------------------------------------------------------------------------------------- + // Error Checking + if (!active_) + { + ROS_ERROR_STREAM_NAMED(name_, "Kinematics not active"); + error_code.val = error_code.NO_IK_SOLUTION; + return false; + } + + if (ik_seed_state.size() != num_joints_) + { + ROS_ERROR_STREAM_NAMED(name_, "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_) + { + ROS_ERROR_STREAM_NAMED(name_, "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; + tf::poseMsgToKDL(ik_pose, frame); + + std::vector vfree(free_params_.size()); + + ros::Time maxTime = ros::Time::now() + ros::Duration(timeout); + int counter = 0; + + double initial_guess = ik_seed_state[free_params_[0]]; + vfree[0] = initial_guess; + + // ------------------------------------------------------------------------------------------------- + // Handle consitency limits if needed + int num_positive_increments; + int num_negative_increments; + + 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 = (int)((max_limit - initial_guess) / search_discretization_); + num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_); + } + else // no consitency 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 + + ROS_DEBUG_STREAM_NAMED(name_, "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) + ROS_WARN_STREAM_ONCE_NAMED(name_, "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; + int numsol = solve(frame, vfree, solutions); + + ROS_DEBUG_STREAM_NAMED(name_, "Found " << numsol << " solutions from IKFast"); + + // ROS_INFO("%f",vfree[0]); + + if (numsol > 0) + { + for (int s = 0; s < numsol; ++s) + { + nattempts++; + std::vector sol; + getSolution(solutions, ik_seed_state, s, sol); + + bool obeys_limits = true; + for (unsigned int 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; + } + // ROS_INFO_STREAM_NAMED(name_,"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.empty()) + { + 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::MoveItErrorCodes::NO_IK_SOLUTION; + break; + } + + vfree[0] = initial_guess + search_discretization_ * counter; + // ROS_DEBUG_STREAM_NAMED(name_,"Attempt " << counter << " with 0th free joint having value " << vfree[0]); + } + + ROS_DEBUG_STREAM_NAMED(name_, "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::MoveItErrorCodes::NO_IK_SOLUTION; + return false; +} + +// Used when there are no redundant joints - aka no free params +bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK"); + + if (!active_) + { + ROS_ERROR("kinematics not active"); + return false; + } + + if (ik_seed_state.size() < num_joints_) + { + ROS_ERROR_STREAM("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)))) + { + ROS_DEBUG_STREAM_NAMED("ikseed", "Not in limits! " << (int)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]; + ROS_ERROR("%u is %f", p, ik_seed_state[p]); // DTC + vfree[i] = ik_seed_state[p]; + } + + KDL::Frame frame; + tf::poseMsgToKDL(ik_pose, frame); + + IkSolutionList solutions; + int numsol = solve(frame, vfree, solutions); + ROS_DEBUG_STREAM_NAMED(name_, "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); + ROS_DEBUG_NAMED(name_, "Sol %d: %e %e %e %e %e %e", (int)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; + ROS_DEBUG_STREAM_NAMED(name_, "Not in limits! " << (int)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 + { + ROS_DEBUG_STREAM_NAMED(name_, "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::MoveItErrorCodes::SUCCESS; + return true; + } + + error_code.val = moveit_msgs::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 +{ + ROS_DEBUG_STREAM_NAMED(name_, "getPositionIK with multiple solutions"); + + if (!active_) + { + ROS_ERROR("kinematics not active"); + result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE; + return false; + } + + if (ik_poses.empty()) + { + ROS_ERROR("ik_poses is empty"); + result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES; + return false; + } + + if (ik_poses.size() > 1) + { + ROS_ERROR("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_) + { + ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires " + << num_joints_); + return false; + } + + KDL::Frame frame; + tf::poseMsgToKDL(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; + ROS_ERROR_STREAM("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); + } + + ROS_DEBUG_STREAM_NAMED(name_, "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; + ROS_DEBUG_STREAM_NAMED(name_, "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 + { + ROS_DEBUG_STREAM_NAMED(name_, "No IK solution"); + } + + result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION; + return false; +} + +bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method, + std::vector& sampled_joint_vals) const +{ + double joint_min = -M_PI; + double joint_max = M_PI; + int index = redundant_joint_indices_.front(); + double joint_dscrt = redundant_joint_discretization_.at(index); + + if (joint_has_limits_vector_[redundant_joint_indices_.front()]) + { + joint_min = joint_min_vector_[index]; + joint_max = joint_max_vector_[index]; + } + + switch (method) + { + case kinematics::DiscretizationMethods::ALL_DISCRETIZED: + { + int steps = std::ceil((joint_max - joint_min) / joint_dscrt); + for (unsigned int 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: + ROS_ERROR_STREAM("Discretization method " << method << " is not supported"); + return false; + } + + return true; +} + +} // end namespace + +// register IKFastKinematicsPlugin as a KinematicsBase implementation +#include +PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase); diff --git a/khi_rs_ikfast_plugin/src/khi_rs030n_manipulator_ikfast_solver.cpp b/khi_rs_ikfast_plugin/src/khi_rs030n_manipulator_ikfast_solver.cpp new file mode 100644 index 0000000..1161517 --- /dev/null +++ b/khi_rs_ikfast_plugin/src/khi_rs030n_manipulator_ikfast_solver.cpp @@ -0,0 +1,12847 @@ +/// 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 0x1000004a generated on 2023-08-30 08:20:06.790960 +/// 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==0x1000004a); + +#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,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42; +x0=IKsin(j[0]); +x1=IKcos(j[3]); +x2=IKcos(j[0]); +x3=IKsin(j[3]); +x4=IKcos(j[2]); +x5=IKsin(j[1]); +x6=IKcos(j[1]); +x7=IKsin(j[2]); +x8=IKsin(j[5]); +x9=IKcos(j[5]); +x10=IKcos(j[4]); +x11=IKsin(j[4]); +x12=((1.0)*x1); +x13=((0.165)*x2); +x14=((1.08)*x2); +x15=((0.165)*x0); +x16=((1.08)*x0); +x17=((1.0)*x3); +x18=((1.0)*x2); +x19=((0.165)*x1); +x20=((1.0)*x0); +x21=(x4*x5); +x22=(x6*x7); +x23=((-1.0)*x11); +x24=((-1.0)*x10); +x25=(x4*x6); +x26=(x0*x5); +x27=(x5*x7); +x28=(x1*x11); +x29=(x17*x2); +x30=((1.0)*x22); +x31=(x18*x22); +x32=((((-1.0)*x30))+x21); +x33=((((-1.0)*x27))+(((-1.0)*x25))); +x34=(x1*x32); +x35=(((x0*x25))+((x26*x7))); +x36=(x10*x34); +x37=(x20*(((((-1.0)*x22))+x21))); +x38=(x18*(((((-1.0)*x27))+(((-1.0)*x25))))); +x39=(x3*x35); +x40=(x1*x38); +x41=(((x11*x37))+((x10*(((((-1.0)*x29))+((x1*x35))))))); +x42=(((x24*(((((-1.0)*x0*x3))+x40))))+((x23*(((((-1.0)*x18*x21))+x31))))); +eerot[0]=(((x41*x8))+((x9*((((x1*x2))+x39))))); +eerot[1]=(((x8*(((((-1.0)*x12*x2))+(((-1.0)*x17*x35))))))+((x41*x9))); +eerot[2]=(((x11*(((((-1.0)*x12*x35))+x29))))+((x10*x37))); +IkReal x43=((1.0)*x22); +eetrans[0]=(((x10*(((((-1.0)*x15*x43))+((x15*x21))))))+(((0.15)*x0))+((x16*x21))+((x11*(((((-1.0)*x19*x35))+((x13*x3))))))+(((-1.0)*x16*x43))+(((0.87)*x26))); +eerot[3]=(((x42*x8))+((x9*(((((-1.0)*x0*x12))+(((-1.0)*x17*x38))))))); +eerot[4]=(((x8*((((x0*x1))+((x3*x38))))))+((x42*x9))); +eerot[5]=(((x11*(((((-1.0)*x0*x17))+x40))))+((x10*(((((-1.0)*x31))+((x2*x21))))))); +IkReal x44=((1.0)*x22); +eetrans[1]=(((x11*((((x19*x38))+(((-1.0)*x15*x3))))))+(((0.87)*x2*x5))+((x10*((((x13*x21))+(((-1.0)*x13*x44))))))+(((0.15)*x2))+((x14*x21))+(((-1.0)*x14*x44))); +eerot[6]=(((x8*(((((-1.0)*x10*x12*x32))+(((-1.0)*x11*x33))))))+((x3*x9*(((((-1.0)*x21))+x30))))); +eerot[7]=(((x9*((((x23*x33))+((x24*x34))))))+((x3*x32*x8))); +eerot[8]=(((x28*x32))+((x10*((x25+x27))))); +eetrans[2]=((0.68)+((x10*(((((0.165)*x27))+(((0.165)*x25))))))+((x28*(((((-0.165)*x22))+(((0.165)*x21))))))+(((1.08)*x25))+(((1.08)*x27))+(((0.87)*x6))); +} + +IKFAST_API int GetNumFreeParameters() { return 0; } +IKFAST_API int* GetFreeParameters() { return NULL; } +IKFAST_API int GetNumJoints() { return 6; } + +IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } + +IKFAST_API int GetIkType() { return 0x67000001; } + +class IKSolver { +public: +IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp; +unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5; + +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; j3=numeric_limits::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; +for(int dummyiter = 0; dummyiter < 1; ++dummyiter) { + solutions.Clear(); +r00 = eerot[0*3+0]; +r01 = eerot[0*3+1]; +r02 = eerot[0*3+2]; +r10 = eerot[1*3+0]; +r11 = eerot[1*3+1]; +r12 = eerot[1*3+2]; +r20 = eerot[2*3+0]; +r21 = eerot[2*3+1]; +r22 = eerot[2*3+2]; +px = eetrans[0]; py = eetrans[1]; pz = eetrans[2]; + +new_r00=r00; +new_r01=r01; +new_r02=r02; +new_px=(px+(((-0.165)*r02))); +new_r10=((-1.0)*r10); +new_r11=((-1.0)*r11); +new_r12=((-1.0)*r12); +new_py=((((0.165)*r12))+(((-1.0)*py))); +new_r20=((-1.0)*r20); +new_r21=((-1.0)*r21); +new_r22=((-1.0)*r22); +new_pz=((0.68)+(((-1.0)*pz))+(((0.165)*r22))); +r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz; +IkReal x45=((1.0)*px); +IkReal x46=((1.0)*pz); +IkReal x47=((1.0)*py); +pp=((px*px)+(py*py)+(pz*pz)); +npx=(((px*r00))+((py*r10))+((pz*r20))); +npy=(((px*r01))+((py*r11))+((pz*r21))); +npz=(((px*r02))+((py*r12))+((pz*r22))); +rxp0_0=((((-1.0)*r20*x47))+((pz*r10))); +rxp0_1=(((px*r20))+(((-1.0)*r00*x46))); +rxp0_2=((((-1.0)*r10*x45))+((py*r00))); +rxp1_0=((((-1.0)*r21*x47))+((pz*r11))); +rxp1_1=(((px*r21))+(((-1.0)*r01*x46))); +rxp1_2=((((-1.0)*r11*x45))+((py*r01))); +rxp2_0=((((-1.0)*r22*x47))+((pz*r12))); +rxp2_1=((((-1.0)*r02*x46))+((px*r22))); +rxp2_2=((((-1.0)*r12*x45))+((py*r02))); +{ +IkReal j0eval[1]; +j0eval[0]=((IKabs(px))+(IKabs(py))); +if( IKabs(j0eval[0]) < 0.0000010000000000 ) +{ +continue; // no branches [j0, j1, j2] + +} else +{ +{ +IkReal j0array[2], cj0array[2], sj0array[2]; +bool j0valid[2]={false}; +_nj0 = 2; +CheckValue x49 = IKatan2WithCheck(IkReal(((-1.0)*px)),IkReal(((-1.0)*py)),IKFAST_ATAN2_MAGTHRESH); +if(!x49.valid){ +continue; +} +IkReal x48=x49.value; +j0array[0]=((-1.0)*x48); +sj0array[0]=IKsin(j0array[0]); +cj0array[0]=IKcos(j0array[0]); +j0array[1]=((3.14159265358979)+(((-1.0)*x48))); +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 j2array[2], cj2array[2], sj2array[2]; +bool j2valid[2]={false}; +_nj2 = 2; +cj2array[0]=((-1.01149425287356)+(((0.532141336739038)*pp))+(((0.159642401021711)*cj0*py))+(((-0.159642401021711)*px*sj0))); +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 j1eval[3]; +IkReal x50=(cj0*py); +IkReal x51=(px*sj0); +IkReal x52=((675.0)*cj2); +IkReal x53=((675.0)*sj2); +j1eval[0]=((-1.02346743295019)+(((-1.0)*cj2))); +j1eval[1]=((IKabs(((81.5625)+(((543.75)*x50))+(((-543.75)*x51))+(((-1.0)*x51*x52))+((x50*x52))+((pz*x53))+(((101.25)*cj2)))))+(IKabs(((((543.75)*pz))+(((-101.25)*sj2))+((x51*x53))+((pz*x52))+(((-1.0)*x50*x53)))))); +j1eval[2]=IKsign(((-1202.0625)+(((-1174.5)*cj2)))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j1eval[3]; +IkReal x54=((27.0)*cj2); +IkReal x55=(cj0*py); +IkReal x56=(px*sj0); +IkReal x57=(pz*sj2); +IkReal x58=((8.27586206896552)*cj2); +IkReal x59=((25.0)*pz); +j1eval[0]=((1.0)+(((-8.27586206896552)*x57))+(((-1.0)*x56*x58))+(((6.66666666666667)*x55))+(((1.24137931034483)*cj2))+((x55*x58))+(((-6.66666666666667)*x56))); +j1eval[1]=((IKabs(((((23.49)*sj2))+(((-1.0)*x55*x59))+(((29.16)*cj2*sj2))+((x56*x59))+(((-3.75)*pz)))))+(IKabs(((-18.9225)+(((-46.98)*cj2))+(((-29.16)*(cj2*cj2)))+((pz*x59)))))); +j1eval[2]=IKsign(((3.2625)+(((-27.0)*x57))+(((21.75)*x55))+(((-1.0)*x54*x56))+(((-21.75)*x56))+(((4.05)*cj2))+((x54*x55)))); +if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j1eval[3]; +IkReal x60=cj0*cj0; +IkReal x61=px*px; +IkReal x62=pz*pz; +IkReal x63=py*py; +IkReal x64=(px*sj0); +IkReal x65=(cj0*py); +IkReal x66=((27.0)*cj2); +IkReal x67=((27.0)*sj2); +IkReal x68=((25.0)*x61); +IkReal x69=((44.4444444444444)*x61); +IkReal x70=(x60*x63); +j1eval[0]=((-1.0)+(((-44.4444444444444)*x62))+((x60*x69))+(((13.3333333333333)*x64))+(((-44.4444444444444)*x70))+(((-13.3333333333333)*x65))+(((-1.0)*x69))+(((88.8888888888889)*x64*x65))); +j1eval[1]=((IKabs(((3.2625)+(((21.75)*x65))+(((-1.0)*x64*x66))+(((-21.75)*x64))+((pz*x67))+((x65*x66))+(((4.05)*cj2)))))+(IKabs(((((-4.05)*sj2))+(((21.75)*pz))+(((-1.0)*x65*x67))+((pz*x66))+((x64*x67)))))); +j1eval[2]=IKsign(((-0.5625)+(((-7.5)*x65))+((x60*x68))+(((50.0)*x64*x65))+(((-25.0)*x70))+(((-1.0)*x68))+(((-25.0)*x62))+(((7.5)*x64)))); +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 x71=px*px; +IkReal x72=cj0*cj0; +IkReal x73=(px*sj0); +IkReal x74=(cj0*py); +IkReal x75=((27.0)*cj2); +IkReal x76=((27.0)*sj2); +IkReal x77=((25.0)*x71); +CheckValue x78 = IKatan2WithCheck(IkReal(((3.2625)+((pz*x76))+(((-21.75)*x73))+(((-1.0)*x73*x75))+(((21.75)*x74))+((x74*x75))+(((4.05)*cj2)))),IkReal((((pz*x75))+(((-4.05)*sj2))+(((21.75)*pz))+(((-1.0)*x74*x76))+((x73*x76)))),IKFAST_ATAN2_MAGTHRESH); +if(!x78.valid){ +continue; +} +CheckValue x79=IKPowWithIntegerCheck(IKsign(((-0.5625)+(((50.0)*x73*x74))+(((-1.0)*x77))+(((-25.0)*(pz*pz)))+(((-25.0)*x72*(py*py)))+((x72*x77))+(((7.5)*x73))+(((-7.5)*x74)))),-1); +if(!x79.valid){ +continue; +} +j1array[0]=((-1.5707963267949)+(x78.value)+(((1.5707963267949)*(x79.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 x80=IKcos(j1); +IkReal x81=IKsin(j1); +IkReal x82=(cj0*py); +IkReal x83=(px*sj0); +IkReal x84=((1.08)*sj2); +IkReal x85=((1.08)*cj2); +IkReal x86=(pz*x80); +IkReal x87=((1.74)*x81); +evalcond[0]=(((x81*x84))+(((0.87)*x80))+pz+((x80*x85))); +evalcond[1]=((0.15)+(((-1.0)*x80*x84))+((x81*x85))+(((0.87)*x81))+x82+(((-1.0)*x83))); +evalcond[2]=((((-1.0)*x80*x82))+((pz*x81))+x84+(((-0.15)*x80))+((x80*x83))); +evalcond[3]=((0.87)+(((0.15)*x81))+((x81*x82))+(((-1.0)*x81*x83))+x86+x85); +evalcond[4]=((0.387)+(((-1.0)*x82*x87))+(((0.3)*x83))+(((-0.261)*x81))+(((-1.74)*x86))+(((-1.0)*pp))+((x83*x87))+(((-0.3)*x82))); +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; +} +} + +rotationfunction0(solutions); +} +} + +} + +} + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +IkReal x863=(px*sj0); +IkReal x864=(cj0*py); +IkReal x865=((27.0)*cj2); +IkReal x866=((25.0)*pz); +CheckValue x867 = IKatan2WithCheck(IkReal(((-18.9225)+(((-46.98)*cj2))+(((-29.16)*(cj2*cj2)))+((pz*x866)))),IkReal(((((23.49)*sj2))+((x863*x866))+(((-1.0)*x864*x866))+(((29.16)*cj2*sj2))+(((-3.75)*pz)))),IKFAST_ATAN2_MAGTHRESH); +if(!x867.valid){ +continue; +} +CheckValue x868=IKPowWithIntegerCheck(IKsign(((3.2625)+(((21.75)*x864))+(((-21.75)*x863))+(((-27.0)*pz*sj2))+((x864*x865))+(((-1.0)*x863*x865))+(((4.05)*cj2)))),-1); +if(!x868.valid){ +continue; +} +j1array[0]=((-1.5707963267949)+(x867.value)+(((1.5707963267949)*(x868.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 x869=IKcos(j1); +IkReal x870=IKsin(j1); +IkReal x871=(cj0*py); +IkReal x872=(px*sj0); +IkReal x873=((1.08)*sj2); +IkReal x874=((1.08)*cj2); +IkReal x875=(pz*x869); +IkReal x876=((1.74)*x870); +evalcond[0]=(((x870*x873))+(((0.87)*x869))+pz+((x869*x874))); +evalcond[1]=((0.15)+((x870*x874))+(((-1.0)*x869*x873))+(((-1.0)*x872))+(((0.87)*x870))+x871); +evalcond[2]=((((-1.0)*x869*x871))+(((-0.15)*x869))+((x869*x872))+x873+((pz*x870))); +evalcond[3]=((0.87)+((x870*x871))+(((-1.0)*x870*x872))+(((0.15)*x870))+x874+x875); +evalcond[4]=((0.387)+(((-0.3)*x871))+(((0.3)*x872))+((x872*x876))+(((-1.0)*x871*x876))+(((-0.261)*x870))+(((-1.0)*pp))+(((-1.74)*x875))); +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; +} +} + +rotationfunction0(solutions); +} +} + +} + +} + +} else +{ +{ +IkReal j1array[1], cj1array[1], sj1array[1]; +bool j1valid[1]={false}; +_nj1 = 1; +IkReal x877=(cj0*py); +IkReal x878=(px*sj0); +IkReal x879=((675.0)*cj2); +IkReal x880=((675.0)*sj2); +CheckValue x881 = IKatan2WithCheck(IkReal(((81.5625)+(((-543.75)*x878))+((pz*x880))+(((543.75)*x877))+(((-1.0)*x878*x879))+((x877*x879))+(((101.25)*cj2)))),IkReal(((((543.75)*pz))+(((-1.0)*x877*x880))+((x878*x880))+(((-101.25)*sj2))+((pz*x879)))),IKFAST_ATAN2_MAGTHRESH); +if(!x881.valid){ +continue; +} +CheckValue x882=IKPowWithIntegerCheck(IKsign(((-1202.0625)+(((-1174.5)*cj2)))),-1); +if(!x882.valid){ +continue; +} +j1array[0]=((-1.5707963267949)+(x881.value)+(((1.5707963267949)*(x882.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 x883=IKcos(j1); +IkReal x884=IKsin(j1); +IkReal x885=(cj0*py); +IkReal x886=(px*sj0); +IkReal x887=((1.08)*sj2); +IkReal x888=((1.08)*cj2); +IkReal x889=(pz*x883); +IkReal x890=((1.74)*x884); +evalcond[0]=(((x883*x888))+pz+(((0.87)*x883))+((x884*x887))); +evalcond[1]=((0.15)+(((-1.0)*x883*x887))+(((-1.0)*x886))+(((0.87)*x884))+x885+((x884*x888))); +evalcond[2]=((((-0.15)*x883))+(((-1.0)*x883*x885))+((pz*x884))+((x883*x886))+x887); +evalcond[3]=((0.87)+(((-1.0)*x884*x886))+(((0.15)*x884))+x889+x888+((x884*x885))); +evalcond[4]=((0.387)+((x886*x890))+(((-1.74)*x889))+(((0.3)*x886))+(((-1.0)*pp))+(((-0.3)*x885))+(((-1.0)*x885*x890))+(((-0.261)*x884))); +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; +} +} + +rotationfunction0(solutions); +} +} + +} + +} +} +} +} +} + +} + +} +} +return solutions.GetNumSolutions()>0; +} +inline void rotationfunction0(IkSolutionListBase& solutions) { +for(int rotationiter = 0; rotationiter < 1; ++rotationiter) { +IkReal x88=((1.0)*sj0); +IkReal x89=(cj0*r12); +IkReal x90=((1.0)*sj1); +IkReal x91=((1.0)*cj1); +IkReal x92=(cj0*r11); +IkReal x93=(cj0*r10); +IkReal x94=(((cj2*sj1))+(((-1.0)*sj2*x91))); +IkReal x95=(((cj1*sj2))+(((-1.0)*cj2*x90))); +IkReal x96=((((-1.0)*sj2*x90))+(((-1.0)*cj2*x91))); +new_r00=(((r10*sj0))+((cj0*r00))); +new_r01=(((r11*sj0))+((cj0*r01))); +new_r02=(((r12*sj0))+((cj0*r02))); +new_r10=(((r20*x94))+(((-1.0)*r00*x88*x96))+((x93*x96))); +new_r11=(((r21*x94))+((x92*x96))+(((-1.0)*r01*x88*x96))); +new_r12=(((x89*x96))+(((-1.0)*r02*x88*x96))+((r22*x94))); +new_r20=(((r20*x96))+(((-1.0)*r00*x88*x95))+((x93*x95))); +new_r21=(((r21*x96))+((x92*x95))+(((-1.0)*r01*x88*x95))); +new_r22=(((x89*x95))+(((-1.0)*r02*x88*x95))+((r22*x96))); +{ +IkReal j4array[2], cj4array[2], sj4array[2]; +bool j4valid[2]={false}; +_nj4 = 2; +cj4array[0]=new_r22; +if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j4valid[0] = j4valid[1] = true; + j4array[0] = IKacos(cj4array[0]); + sj4array[0] = IKsin(j4array[0]); + cj4array[1] = cj4array[0]; + j4array[1] = -j4array[0]; + sj4array[1] = -sj4array[0]; +} +else if( isnan(cj4array[0]) ) +{ + // probably any value will work + j4valid[0] = true; + cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0; +} +for(int ij4 = 0; ij4 < 2; ++ij4) +{ +if( !j4valid[ij4] ) +{ + continue; +} +_ij4[0] = ij4; _ij4[1] = -1; +for(int iij4 = ij4+1; iij4 < 2; ++iij4) +{ +if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH ) +{ + j4valid[iij4]=false; _ij4[1] = iij4; break; +} +} +j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4]; + +{ +IkReal j5eval[3]; +j5eval[0]=sj4; +j5eval[1]=IKsign(sj4); +j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +j3eval[0]=sj4; +j3eval[1]=IKsign(sj4); +j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[2]; +j3eval[0]=new_r02; +j3eval[1]=sj4; +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[5]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +IkReal j5mul = 1; +j5=0; +j3mul=-1.0; +if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3=IKatan2(((-1.0)*new_r01), new_r00); +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].fmul = j3mul; +vinfos[3].freeind = 0; +vinfos[3].maxsolutions = 0; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].fmul = j5mul; +vinfos[5].freeind = 0; +vinfos[5].maxsolutions = 0; +std::vector vfree(1); +vfree[0] = 5; +solutions.AddSolution(vinfos,vfree); +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +IkReal j5mul = 1; +j5=0; +j3mul=1.0; +if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3=IKatan2(new_r01, ((-1.0)*new_r11)); +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].fmul = j3mul; +vinfos[3].freeind = 0; +vinfos[3].maxsolutions = 0; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].fmul = j5mul; +vinfos[5].freeind = 0; +vinfos[5].maxsolutions = 0; +std::vector vfree(1); +vfree[0] = 5; +solutions.AddSolution(vinfos,vfree); +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +IkReal x97=new_r22*new_r22; +IkReal x98=((16.0)*new_r10); +IkReal x99=((16.0)*new_r01); +IkReal x100=((16.0)*new_r00); +IkReal x101=(new_r11*new_r22); +IkReal x102=((8.0)*new_r00); +IkReal x103=(x97*x98); +IkReal x104=(x97*x99); +j3eval[0]=((IKabs((((new_r22*x102))+(((-8.0)*new_r11)))))+(IKabs(((((32.0)*new_r00))+(((-16.0)*x101))+(((-1.0)*x100*x97)))))+(IKabs((((x102*x97))+(((-8.0)*x101)))))+(IKabs((x104+(((-1.0)*x99)))))+(IKabs(((((-1.0)*x104))+x99)))+(IKabs(((((-32.0)*new_r11*x97))+((new_r22*x100))+(((16.0)*new_r11)))))+(IKabs(((((-1.0)*x103))+x98)))+(IKabs((x103+(((-1.0)*x98)))))); +if( IKabs(j3eval[0]) < 0.0000000100000000 ) +{ +continue; // no branches [j3, j5] + +} else +{ +IkReal op[4+1], zeror[4]; +int numroots; +IkReal j3evalpoly[1]; +IkReal x105=new_r22*new_r22; +IkReal x106=((16.0)*new_r01); +IkReal x107=(new_r00*new_r22); +IkReal x108=(x105*x106); +IkReal x109=((((8.0)*x107))+(((-8.0)*new_r11))); +op[0]=x109; +op[1]=((((-1.0)*x106))+x108); +op[2]=((((-32.0)*new_r11*x105))+(((16.0)*x107))+(((16.0)*new_r11))); +op[3]=((((-1.0)*x108))+x106); +op[4]=x109; +polyroots4(op,zeror,numroots); +IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1]; +int numsolutions = 0; +for(int ij3 = 0; ij3 < numroots; ++ij3) +{ +IkReal htj3 = zeror[ij3]; +tempj3array[0]=((2.0)*(atan(htj3))); +for(int kj3 = 0; kj3 < 1; ++kj3) +{ +j3array[numsolutions] = tempj3array[kj3]; +if( j3array[numsolutions] > IKPI ) +{ + j3array[numsolutions]-=IK2PI; +} +else if( j3array[numsolutions] < -IKPI ) +{ + j3array[numsolutions]+=IK2PI; +} +sj3array[numsolutions] = IKsin(j3array[numsolutions]); +cj3array[numsolutions] = IKcos(j3array[numsolutions]); +numsolutions++; +} +} +bool j3valid[4]={true,true,true,true}; +_nj3 = 4; +for(int ij3 = 0; ij3 < numsolutions; ++ij3) + { +if( !j3valid[ij3] ) +{ + continue; +} + j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +htj3 = IKtan(j3/2); + +IkReal x110=new_r22*new_r22; +IkReal x111=((16.0)*new_r10); +IkReal x112=(new_r11*new_r22); +IkReal x113=((8.0)*x112); +IkReal x114=(new_r00*x110); +IkReal x115=(x110*x111); +IkReal x116=((8.0)*x114); +j3evalpoly[0]=((((htj3*htj3)*(((((32.0)*new_r00))+(((-16.0)*x114))+(((-16.0)*x112))))))+(((htj3*htj3*htj3)*(((((-1.0)*x115))+x111))))+(((-1.0)*x113))+x116+((htj3*(((((-1.0)*x111))+x115))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x113))+x116))))); +if( IKabs(j3evalpoly[0]) > 0.0000001000000000 ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +{ +IkReal j5eval[3]; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +IkReal x117=cj3*cj3; +IkReal x118=new_r22*new_r22; +IkReal x119=(new_r22*sj3); +IkReal x120=((((-1.0)*x117))+(((-1.0)*x118))+((x117*x118))); +j5eval[0]=x120; +j5eval[1]=IKsign(x120); +j5eval[2]=((IKabs((((new_r01*x119))+(((-1.0)*cj3*new_r00)))))+(IKabs((((new_r00*x119))+((cj3*new_r01)))))); +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[1]; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +j5eval[0]=new_r22; +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[1]; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +j5eval[0]=cj3; +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r11), new_r10); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[4]; +IkReal x121=IKsin(j5); +IkReal x122=IKcos(j5); +evalcond[0]=(x121+new_r11); +evalcond[1]=((-1.0)*x121); +evalcond[2]=((-1.0)*x122); +evalcond[3]=((((-1.0)*x122))+new_r10); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[4]; +IkReal x123=IKsin(j5); +IkReal x124=IKcos(j5); +evalcond[0]=((-1.0)*x123); +evalcond[1]=((-1.0)*x124); +evalcond[2]=(x123+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x124))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +IkReal x125=new_r22*new_r22; +CheckValue x126=IKPowWithIntegerCheck(((-1.0)+x125),-1); +if(!x126.valid){ +continue; +} +if(((x125*(x126.value))) < -0.00001) +continue; +IkReal gconst12=IKsqrt((x125*(x126.value))); +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst12)))))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[1]; +IkReal x127=new_r22*new_r22; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +sj3=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))); +cj3=gconst12; +if( (gconst12) < -1-IKFAST_SINCOS_THRESH || (gconst12) > 1+IKFAST_SINCOS_THRESH ) + continue; +j3=IKacos(gconst12); +CheckValue x128=IKPowWithIntegerCheck(((-1.0)+x127),-1); +if(!x128.valid){ +continue; +} +if(((x127*(x128.value))) < -0.00001) +continue; +IkReal gconst12=IKsqrt((x127*(x128.value))); +j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01))); +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +CheckValue x129=IKPowWithIntegerCheck(gconst12,-1); +if(!x129.valid){ +continue; +} +if( IKabs(((((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x129.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))))+IKsqr((new_r00*(x129.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))), (new_r00*(x129.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x130=IKsin(j5); +IkReal x131=IKcos(j5); +IkReal x132=((1.0)*x131); +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +IkReal x133=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))); +evalcond[0]=((-1.0)*x130); +evalcond[1]=((-1.0)*x131); +evalcond[2]=(((gconst12*x130))+new_r01); +evalcond[3]=((((-1.0)*gconst12*x132))+new_r00); +evalcond[4]=(((x130*x133))+new_r11); +evalcond[5]=((((-1.0)*x132*x133))+new_r10); +evalcond[6]=(((gconst12*new_r01))+x130+((new_r11*x133))); +evalcond[7]=(((gconst12*new_r00))+(((-1.0)*x132))+((new_r10*x133))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x134 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH); +if(!x134.valid){ +continue; +} +CheckValue x135=IKPowWithIntegerCheck(IKsign(gconst12),-1); +if(!x135.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(x134.value)+(((1.5707963267949)*(x135.value)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x136=IKsin(j5); +IkReal x137=IKcos(j5); +IkReal x138=((1.0)*x137); +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +IkReal x139=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))); +evalcond[0]=((-1.0)*x136); +evalcond[1]=((-1.0)*x137); +evalcond[2]=(((gconst12*x136))+new_r01); +evalcond[3]=((((-1.0)*gconst12*x138))+new_r00); +evalcond[4]=(((x136*x139))+new_r11); +evalcond[5]=((((-1.0)*x138*x139))+new_r10); +evalcond[6]=(((gconst12*new_r01))+x136+((new_r11*x139))); +evalcond[7]=(((gconst12*new_r00))+(((-1.0)*x138))+((new_r10*x139))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +IkReal x140=new_r22*new_r22; +CheckValue x141=IKPowWithIntegerCheck(((-1.0)+x140),-1); +if(!x141.valid){ +continue; +} +if(((x140*(x141.value))) < -0.00001) +continue; +IkReal gconst12=IKsqrt((x140*(x141.value))); +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst12)))))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[1]; +IkReal x142=new_r22*new_r22; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))); +cj3=gconst12; +if( (gconst12) < -1-IKFAST_SINCOS_THRESH || (gconst12) > 1+IKFAST_SINCOS_THRESH ) + continue; +j3=((-1.0)*(IKacos(gconst12))); +CheckValue x143=IKPowWithIntegerCheck(((-1.0)+x142),-1); +if(!x143.valid){ +continue; +} +if(((x142*(x143.value))) < -0.00001) +continue; +IkReal gconst12=IKsqrt((x142*(x143.value))); +j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01))); +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +CheckValue x144=IKPowWithIntegerCheck(gconst12,-1); +if(!x144.valid){ +continue; +} +if( IKabs((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x144.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))))+IKsqr((new_r00*(x144.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2((((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))))))+(((-1.0)*gconst12*new_r01))), (new_r00*(x144.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x145=IKsin(j5); +IkReal x146=IKcos(j5); +IkReal x147=((1.0)*x146); +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +IkReal x148=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))); +IkReal x149=((1.0)*x148); +evalcond[0]=((-1.0)*x145); +evalcond[1]=((-1.0)*x146); +evalcond[2]=(((gconst12*x145))+new_r01); +evalcond[3]=((((-1.0)*gconst12*x147))+new_r00); +evalcond[4]=(((x146*x148))+new_r10); +evalcond[5]=((((-1.0)*x145*x149))+new_r11); +evalcond[6]=(((gconst12*new_r01))+(((-1.0)*new_r11*x149))+x145); +evalcond[7]=((((-1.0)*x147))+((gconst12*new_r00))+(((-1.0)*new_r10*x149))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x150 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH); +if(!x150.valid){ +continue; +} +CheckValue x151=IKPowWithIntegerCheck(IKsign(gconst12),-1); +if(!x151.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(x150.value)+(((1.5707963267949)*(x151.value)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x152=IKsin(j5); +IkReal x153=IKcos(j5); +IkReal x154=((1.0)*x153); +if((((1.0)+(((-1.0)*(gconst12*gconst12))))) < -0.00001) +continue; +IkReal x155=IKsqrt(((1.0)+(((-1.0)*(gconst12*gconst12))))); +IkReal x156=((1.0)*x155); +evalcond[0]=((-1.0)*x152); +evalcond[1]=((-1.0)*x153); +evalcond[2]=(((gconst12*x152))+new_r01); +evalcond[3]=(new_r00+(((-1.0)*gconst12*x154))); +evalcond[4]=(((x153*x155))+new_r10); +evalcond[5]=((((-1.0)*x152*x156))+new_r11); +evalcond[6]=(((gconst12*new_r01))+x152+(((-1.0)*new_r11*x156))); +evalcond[7]=(((gconst12*new_r00))+(((-1.0)*x154))+(((-1.0)*new_r10*x156))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +IkReal x157=new_r22*new_r22; +CheckValue x158=IKPowWithIntegerCheck(((-1.0)+x157),-1); +if(!x158.valid){ +continue; +} +if(((x157*(x158.value))) < -0.00001) +continue; +IkReal gconst13=((-1.0)*(IKsqrt((x157*(x158.value))))); +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst13)))))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[1]; +IkReal x159=new_r22*new_r22; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +sj3=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))); +cj3=gconst13; +if( (gconst13) < -1-IKFAST_SINCOS_THRESH || (gconst13) > 1+IKFAST_SINCOS_THRESH ) + continue; +j3=IKacos(gconst13); +CheckValue x160=IKPowWithIntegerCheck(((-1.0)+x159),-1); +if(!x160.valid){ +continue; +} +if(((x159*(x160.value))) < -0.00001) +continue; +IkReal gconst13=((-1.0)*(IKsqrt((x159*(x160.value))))); +j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01))); +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +CheckValue x161=IKPowWithIntegerCheck(gconst13,-1); +if(!x161.valid){ +continue; +} +if( IKabs(((((-1.0)*gconst13*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x161.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst13*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))))+IKsqr((new_r00*(x161.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*gconst13*new_r01))+(((-1.0)*new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))), (new_r00*(x161.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x162=IKsin(j5); +IkReal x163=IKcos(j5); +IkReal x164=((1.0)*x163); +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +IkReal x165=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))); +evalcond[0]=((-1.0)*x162); +evalcond[1]=((-1.0)*x163); +evalcond[2]=(new_r01+((gconst13*x162))); +evalcond[3]=(new_r00+(((-1.0)*gconst13*x164))); +evalcond[4]=(((x162*x165))+new_r11); +evalcond[5]=(new_r10+(((-1.0)*x164*x165))); +evalcond[6]=(((new_r11*x165))+x162+((gconst13*new_r01))); +evalcond[7]=((((-1.0)*x164))+((new_r10*x165))+((gconst13*new_r00))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x166 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH); +if(!x166.valid){ +continue; +} +CheckValue x167=IKPowWithIntegerCheck(IKsign(gconst13),-1); +if(!x167.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(x166.value)+(((1.5707963267949)*(x167.value)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x168=IKsin(j5); +IkReal x169=IKcos(j5); +IkReal x170=((1.0)*x169); +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +IkReal x171=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))); +evalcond[0]=((-1.0)*x168); +evalcond[1]=((-1.0)*x169); +evalcond[2]=(new_r01+((gconst13*x168))); +evalcond[3]=((((-1.0)*gconst13*x170))+new_r00); +evalcond[4]=(((x168*x171))+new_r11); +evalcond[5]=((((-1.0)*x170*x171))+new_r10); +evalcond[6]=(((new_r11*x171))+x168+((gconst13*new_r01))); +evalcond[7]=(((new_r10*x171))+(((-1.0)*x170))+((gconst13*new_r00))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +IkReal x172=new_r22*new_r22; +CheckValue x173=IKPowWithIntegerCheck(((-1.0)+x172),-1); +if(!x173.valid){ +continue; +} +if(((x172*(x173.value))) < -0.00001) +continue; +IkReal gconst13=((-1.0)*(IKsqrt((x172*(x173.value))))); +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((cj3+(((-1.0)*gconst13)))))+(IKabs(((1.0)+(IKsign(sj3)))))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[1]; +IkReal x174=new_r22*new_r22; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))); +cj3=gconst13; +if( (gconst13) < -1-IKFAST_SINCOS_THRESH || (gconst13) > 1+IKFAST_SINCOS_THRESH ) + continue; +j3=((-1.0)*(IKacos(gconst13))); +CheckValue x175=IKPowWithIntegerCheck(((-1.0)+x174),-1); +if(!x175.valid){ +continue; +} +if(((x174*(x175.value))) < -0.00001) +continue; +IkReal gconst13=((-1.0)*(IKsqrt((x174*(x175.value))))); +j5eval[0]=((IKabs(new_r00))+(IKabs(new_r01))); +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +CheckValue x176=IKPowWithIntegerCheck(gconst13,-1); +if(!x176.valid){ +continue; +} +if( IKabs(((((-1.0)*gconst13*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x176.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*gconst13*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))))+IKsqr((new_r00*(x176.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*gconst13*new_r01))+((new_r11*(IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))))))), (new_r00*(x176.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x177=IKsin(j5); +IkReal x178=IKcos(j5); +IkReal x179=((1.0)*x178); +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +IkReal x180=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))); +IkReal x181=((1.0)*x180); +evalcond[0]=((-1.0)*x177); +evalcond[1]=((-1.0)*x178); +evalcond[2]=(new_r01+((gconst13*x177))); +evalcond[3]=((((-1.0)*gconst13*x179))+new_r00); +evalcond[4]=(((x178*x180))+new_r10); +evalcond[5]=((((-1.0)*x177*x181))+new_r11); +evalcond[6]=(x177+(((-1.0)*new_r11*x181))+((gconst13*new_r01))); +evalcond[7]=((((-1.0)*x179))+((gconst13*new_r00))+(((-1.0)*new_r10*x181))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x182 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH); +if(!x182.valid){ +continue; +} +CheckValue x183=IKPowWithIntegerCheck(IKsign(gconst13),-1); +if(!x183.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(x182.value)+(((1.5707963267949)*(x183.value)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x184=IKsin(j5); +IkReal x185=IKcos(j5); +IkReal x186=((1.0)*x185); +if((((1.0)+(((-1.0)*(gconst13*gconst13))))) < -0.00001) +continue; +IkReal x187=IKsqrt(((1.0)+(((-1.0)*(gconst13*gconst13))))); +IkReal x188=((1.0)*x187); +evalcond[0]=((-1.0)*x184); +evalcond[1]=((-1.0)*x185); +evalcond[2]=(((gconst13*x184))+new_r01); +evalcond[3]=((((-1.0)*gconst13*x186))+new_r00); +evalcond[4]=(((x185*x187))+new_r10); +evalcond[5]=((((-1.0)*x184*x188))+new_r11); +evalcond[6]=(x184+(((-1.0)*new_r11*x188))+((gconst13*new_r01))); +evalcond[7]=((((-1.0)*x186))+((gconst13*new_r00))+(((-1.0)*new_r10*x188))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j5] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x189=((1.0)*new_r22); +IkReal x190=(cj3*new_r01); +CheckValue x191=IKPowWithIntegerCheck(cj3,-1); +if(!x191.valid){ +continue; +} +if( IKabs(((((-1.0)*x190))+(((-1.0)*new_r11*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x191.value)*(((((-1.0)*new_r11*x189))+((new_r11*new_r22*(cj3*cj3)))+new_r00+(((-1.0)*sj3*x189*x190)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x190))+(((-1.0)*new_r11*sj3))))+IKsqr(((x191.value)*(((((-1.0)*new_r11*x189))+((new_r11*new_r22*(cj3*cj3)))+new_r00+(((-1.0)*sj3*x189*x190))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*x190))+(((-1.0)*new_r11*sj3))), ((x191.value)*(((((-1.0)*new_r11*x189))+((new_r11*new_r22*(cj3*cj3)))+new_r00+(((-1.0)*sj3*x189*x190)))))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[10]; +IkReal x192=IKsin(j5); +IkReal x193=IKcos(j5); +IkReal x194=((1.0)*cj3); +IkReal x195=((1.0)*sj3); +IkReal x196=(cj3*new_r10); +IkReal x197=(cj3*new_r11); +IkReal x198=((1.0)*x193); +IkReal x199=(new_r22*x193); +IkReal x200=(sj3*x192); +IkReal x201=(new_r22*x192); +evalcond[0]=(((new_r11*sj3))+x192+((cj3*new_r01))); +evalcond[1]=(((new_r10*sj3))+(((-1.0)*x198))+((cj3*new_r00))); +evalcond[2]=(((sj3*x199))+((cj3*x192))+new_r01); +evalcond[3]=(((new_r22*x200))+new_r00+(((-1.0)*x193*x194))); +evalcond[4]=((((-1.0)*x194*x199))+x200+new_r11); +evalcond[5]=(x196+(((-1.0)*new_r00*x195))+(((-1.0)*x201))); +evalcond[6]=((((-1.0)*new_r22*x198))+x197+(((-1.0)*new_r01*x195))); +evalcond[7]=((((-1.0)*x194*x201))+new_r10+(((-1.0)*x193*x195))); +evalcond[8]=((((-1.0)*new_r00*new_r22*x195))+(((-1.0)*x192))+((new_r22*x196))); +evalcond[9]=((((-1.0)*x198))+((new_r22*x197))+(((-1.0)*new_r01*new_r22*x195))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x202=((1.0)*new_r01); +CheckValue x203=IKPowWithIntegerCheck(new_r22,-1); +if(!x203.valid){ +continue; +} +if( IKabs(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x202)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x203.value)*((((cj3*new_r11))+(((-1.0)*sj3*x202)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x202))))+IKsqr(((x203.value)*((((cj3*new_r11))+(((-1.0)*sj3*x202))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*new_r11*sj3))+(((-1.0)*cj3*x202))), ((x203.value)*((((cj3*new_r11))+(((-1.0)*sj3*x202)))))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[10]; +IkReal x204=IKsin(j5); +IkReal x205=IKcos(j5); +IkReal x206=((1.0)*cj3); +IkReal x207=((1.0)*sj3); +IkReal x208=(cj3*new_r10); +IkReal x209=(cj3*new_r11); +IkReal x210=((1.0)*x205); +IkReal x211=(new_r22*x205); +IkReal x212=(sj3*x204); +IkReal x213=(new_r22*x204); +evalcond[0]=(((new_r11*sj3))+x204+((cj3*new_r01))); +evalcond[1]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x210))); +evalcond[2]=(((cj3*x204))+((sj3*x211))+new_r01); +evalcond[3]=(((new_r22*x212))+(((-1.0)*x205*x206))+new_r00); +evalcond[4]=((((-1.0)*x206*x211))+x212+new_r11); +evalcond[5]=((((-1.0)*new_r00*x207))+x208+(((-1.0)*x213))); +evalcond[6]=(x209+(((-1.0)*new_r22*x210))+(((-1.0)*new_r01*x207))); +evalcond[7]=((((-1.0)*x206*x213))+(((-1.0)*x205*x207))+new_r10); +evalcond[8]=(((new_r22*x208))+(((-1.0)*new_r00*new_r22*x207))+(((-1.0)*x204))); +evalcond[9]=((((-1.0)*new_r01*new_r22*x207))+((new_r22*x209))+(((-1.0)*x210))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x214=cj3*cj3; +IkReal x215=new_r22*new_r22; +IkReal x216=(new_r22*sj3); +CheckValue x217=IKPowWithIntegerCheck(IKsign((((x214*x215))+(((-1.0)*x214))+(((-1.0)*x215)))),-1); +if(!x217.valid){ +continue; +} +CheckValue x218 = IKatan2WithCheck(IkReal((((new_r00*x216))+((cj3*new_r01)))),IkReal(((((-1.0)*cj3*new_r00))+((new_r01*x216)))),IKFAST_ATAN2_MAGTHRESH); +if(!x218.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x217.value)))+(x218.value)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[10]; +IkReal x219=IKsin(j5); +IkReal x220=IKcos(j5); +IkReal x221=((1.0)*cj3); +IkReal x222=((1.0)*sj3); +IkReal x223=(cj3*new_r10); +IkReal x224=(cj3*new_r11); +IkReal x225=((1.0)*x220); +IkReal x226=(new_r22*x220); +IkReal x227=(sj3*x219); +IkReal x228=(new_r22*x219); +evalcond[0]=(((new_r11*sj3))+x219+((cj3*new_r01))); +evalcond[1]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x225))); +evalcond[2]=(((cj3*x219))+((sj3*x226))+new_r01); +evalcond[3]=((((-1.0)*x220*x221))+((new_r22*x227))+new_r00); +evalcond[4]=(x227+(((-1.0)*x221*x226))+new_r11); +evalcond[5]=(x223+(((-1.0)*new_r00*x222))+(((-1.0)*x228))); +evalcond[6]=((((-1.0)*new_r01*x222))+x224+(((-1.0)*new_r22*x225))); +evalcond[7]=((((-1.0)*x220*x222))+(((-1.0)*x221*x228))+new_r10); +evalcond[8]=((((-1.0)*new_r00*new_r22*x222))+(((-1.0)*x219))+((new_r22*x223))); +evalcond[9]=((((-1.0)*new_r01*new_r22*x222))+((new_r22*x224))+(((-1.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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + } + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j3, j5] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x230=IKPowWithIntegerCheck(sj4,-1); +if(!x230.valid){ +continue; +} +IkReal x229=x230.value; +CheckValue x231=IKPowWithIntegerCheck(new_r02,-1); +if(!x231.valid){ +continue; +} +if( IKabs((x229*(x231.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r12*new_r12))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r12*x229)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x229*(x231.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r12*new_r12)))))))+IKsqr(((-1.0)*new_r12*x229))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((x229*(x231.value)*(((1.0)+(((-1.0)*(cj4*cj4)))+(((-1.0)*(new_r12*new_r12)))))), ((-1.0)*new_r12*x229)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x232=IKsin(j3); +IkReal x233=IKcos(j3); +IkReal x234=((1.0)*cj4); +IkReal x235=((1.0)*sj4); +IkReal x236=(new_r02*x232); +IkReal x237=(new_r12*x233); +IkReal x238=(sj4*x233); +evalcond[0]=(x238+new_r12); +evalcond[1]=(new_r02+(((-1.0)*x232*x235))); +evalcond[2]=(((new_r02*x233))+((new_r12*x232))); +evalcond[3]=(sj4+x237+(((-1.0)*x236))); +evalcond[4]=(((new_r22*sj4))+((cj4*x237))+(((-1.0)*x234*x236))); +evalcond[5]=(((new_r10*x238))+(((-1.0)*new_r20*x234))+(((-1.0)*new_r00*x232*x235))); +evalcond[6]=((((-1.0)*new_r01*x232*x235))+(((-1.0)*new_r21*x234))+((new_r11*x238))); +evalcond[7]=((1.0)+(((-1.0)*new_r22*x234))+(((-1.0)*x235*x236))+((sj4*x237))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j5eval[3]; +j5eval[0]=sj4; +j5eval[1]=IKsign(sj4); +j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[2]; +j5eval[0]=sj4; +j5eval[1]=cj3; +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[3]; +j5eval[0]=sj4; +j5eval[1]=cj4; +j5eval[2]=sj3; +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[5]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x239=((1.0)*new_r01); +if( IKabs(((((-1.0)*cj3*x239))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r00))+(((-1.0)*sj3*x239)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x239))+(((-1.0)*new_r00*sj3))))+IKsqr((((cj3*new_r00))+(((-1.0)*sj3*x239))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*cj3*x239))+(((-1.0)*new_r00*sj3))), (((cj3*new_r00))+(((-1.0)*sj3*x239)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x240=IKsin(j5); +IkReal x241=IKcos(j5); +IkReal x242=((1.0)*sj3); +IkReal x243=(sj3*x240); +IkReal x244=((1.0)*x241); +IkReal x245=((1.0)*x240); +IkReal x246=(cj3*x244); +evalcond[0]=(((new_r11*sj3))+x240+((cj3*new_r01))); +evalcond[1]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x244))); +evalcond[2]=(((sj3*x241))+new_r01+((cj3*x240))); +evalcond[3]=(x243+(((-1.0)*x246))+new_r00); +evalcond[4]=(x243+(((-1.0)*x246))+new_r11); +evalcond[5]=((((-1.0)*new_r00*x242))+((cj3*new_r10))+(((-1.0)*x245))); +evalcond[6]=((((-1.0)*new_r01*x242))+((cj3*new_r11))+(((-1.0)*x244))); +evalcond[7]=((((-1.0)*x241*x242))+(((-1.0)*cj3*x245))+new_r10); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x247=((1.0)*cj3); +if( IKabs(((((-1.0)*new_r01*x247))+(((-1.0)*new_r11*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((new_r01*sj3))+(((-1.0)*new_r11*x247)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r01*x247))+(((-1.0)*new_r11*sj3))))+IKsqr((((new_r01*sj3))+(((-1.0)*new_r11*x247))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*new_r01*x247))+(((-1.0)*new_r11*sj3))), (((new_r01*sj3))+(((-1.0)*new_r11*x247)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x248=IKsin(j5); +IkReal x249=IKcos(j5); +IkReal x250=((1.0)*sj3); +IkReal x251=(cj3*x248); +IkReal x252=((1.0)*x249); +IkReal x253=(x249*x250); +evalcond[0]=(((new_r11*sj3))+x248+((cj3*new_r01))); +evalcond[1]=(x248+((cj3*new_r10))+(((-1.0)*new_r00*x250))); +evalcond[2]=((((-1.0)*new_r01*x250))+x249+((cj3*new_r11))); +evalcond[3]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x252))); +evalcond[4]=(((sj3*x248))+new_r11+((cj3*x249))); +evalcond[5]=(x251+new_r01+(((-1.0)*x253))); +evalcond[6]=(x251+new_r10+(((-1.0)*x253))); +evalcond[7]=((((-1.0)*x248*x250))+(((-1.0)*cj3*x252))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r20, new_r21); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x254=IKsin(j5); +IkReal x255=IKcos(j5); +IkReal x256=((1.0)*x255); +IkReal x257=((1.0)*x254); +evalcond[0]=(new_r20+(((-1.0)*x257))); +evalcond[1]=(new_r21+(((-1.0)*x256))); +evalcond[2]=(((sj3*x254))+new_r11); +evalcond[3]=(new_r01+(((-1.0)*new_r12*x257))); +evalcond[4]=((((-1.0)*cj3*x256))+new_r00); +evalcond[5]=((((-1.0)*sj3*x256))+new_r10); +evalcond[6]=(((new_r11*sj3))+x254+((cj3*new_r01))); +evalcond[7]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x256))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20))+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r20), ((-1.0)*new_r21)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x258=IKsin(j5); +IkReal x259=IKcos(j5); +IkReal x260=((1.0)*x259); +evalcond[0]=(x258+new_r20); +evalcond[1]=(x259+new_r21); +evalcond[2]=(((new_r12*x258))+new_r01); +evalcond[3]=(((sj3*x258))+new_r11); +evalcond[4]=((((-1.0)*cj3*x260))+new_r00); +evalcond[5]=((((-1.0)*sj3*x260))+new_r10); +evalcond[6]=(((new_r11*sj3))+x258+((cj3*new_r01))); +evalcond[7]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x260))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959))); +evalcond[1]=new_r02; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r01), new_r00); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x261=IKsin(j5); +IkReal x262=IKcos(j5); +IkReal x263=((1.0)*cj4); +IkReal x264=((1.0)*x262); +IkReal x265=((1.0)*x261); +evalcond[0]=(x261+new_r01); +evalcond[1]=(new_r00+(((-1.0)*x264))); +evalcond[2]=((((-1.0)*sj4*x265))+new_r20); +evalcond[3]=((((-1.0)*sj4*x264))+new_r21); +evalcond[4]=((((-1.0)*x262*x263))+new_r11); +evalcond[5]=((((-1.0)*x261*x263))+new_r10); +evalcond[6]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x265))); +evalcond[7]=(((cj4*new_r11))+((new_r21*sj4))+(((-1.0)*x264))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +evalcond[1]=new_r02; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r01, ((-1.0)*new_r00)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x266=IKsin(j5); +IkReal x267=IKcos(j5); +IkReal x268=((1.0)*cj4); +IkReal x269=((1.0)*x267); +IkReal x270=((1.0)*x266); +evalcond[0]=(x266+(((-1.0)*new_r01))); +evalcond[1]=(((cj4*x267))+new_r11); +evalcond[2]=((((-1.0)*sj4*x270))+new_r20); +evalcond[3]=((((-1.0)*sj4*x269))+new_r21); +evalcond[4]=((((-1.0)*new_r00))+(((-1.0)*x269))); +evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x266*x268))); +evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r10*x268))+(((-1.0)*x270))); +evalcond[7]=((((-1.0)*new_r11*x268))+((new_r21*sj4))+(((-1.0)*x269))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +evalcond[1]=new_r12; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r11), new_r10); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x271=IKcos(j5); +IkReal x272=IKsin(j5); +IkReal x273=((1.0)*cj4); +IkReal x274=((1.0)*x271); +IkReal x275=((1.0)*x272); +evalcond[0]=(x272+new_r11); +evalcond[1]=((((-1.0)*x274))+new_r10); +evalcond[2]=(((cj4*x271))+new_r01); +evalcond[3]=(((cj4*x272))+new_r00); +evalcond[4]=((((-1.0)*new_r02*x275))+new_r20); +evalcond[5]=((((-1.0)*new_r02*x274))+new_r21); +evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r00*x273))+(((-1.0)*x275))); +evalcond[7]=((((-1.0)*new_r01*x273))+(((-1.0)*x274))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +evalcond[1]=new_r12; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[3]; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +j5eval[0]=new_r02; +j5eval[1]=IKsign(new_r02); +j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[1]; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +j5eval[0]=new_r02; +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[2]; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +j5eval[0]=new_r02; +j5eval[1]=cj4; +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[4]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +evalcond[2]=new_r01; +evalcond[3]=new_r00; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r20, new_r21); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[4]; +IkReal x276=IKsin(j5); +IkReal x277=((1.0)*(IKcos(j5))); +evalcond[0]=((((-1.0)*x276))+new_r20); +evalcond[1]=((((-1.0)*x277))+new_r21); +evalcond[2]=(x276+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x277))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +evalcond[2]=new_r01; +evalcond[3]=new_r00; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[4]; +IkReal x278=IKsin(j5); +IkReal x279=IKcos(j5); +evalcond[0]=(x278+new_r20); +evalcond[1]=(x279+new_r21); +evalcond[2]=(x278+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x279))+(((-1.0)*new_r10))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=IKabs(new_r02); +evalcond[1]=new_r20; +evalcond[2]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[6]; +IkReal x280=IKsin(j5); +IkReal x281=IKcos(j5); +IkReal x282=((1.0)*cj4); +IkReal x283=((1.0)*x281); +evalcond[0]=(x280+(((-1.0)*new_r11))); +evalcond[1]=((((-1.0)*x281*x282))+new_r01); +evalcond[2]=((((-1.0)*x280*x282))+new_r00); +evalcond[3]=((((-1.0)*new_r10))+(((-1.0)*x283))); +evalcond[4]=(((cj4*new_r00))+(((-1.0)*x280))); +evalcond[5]=(((cj4*new_r01))+(((-1.0)*x283))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[6]; +IkReal x284=IKcos(j5); +IkReal x285=IKsin(j5); +IkReal x286=((-1.0)*x285); +IkReal x287=((-1.0)*x284); +evalcond[0]=x286; +evalcond[1]=x287; +evalcond[2]=(new_r22*x287); +evalcond[3]=(new_r22*x286); +evalcond[4]=(x285+(((-1.0)*new_r11))); +evalcond[5]=((((-1.0)*x284))+(((-1.0)*new_r10))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j5] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x288=IKPowWithIntegerCheck(new_r02,-1); +if(!x288.valid){ +continue; +} +CheckValue x289=IKPowWithIntegerCheck(cj4,-1); +if(!x289.valid){ +continue; +} +if( IKabs(((-1.0)*new_r20*(x288.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x289.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20*(x288.value)))+IKsqr((new_r01*(x289.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r20*(x288.value)), (new_r01*(x289.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x290=IKsin(j5); +IkReal x291=IKcos(j5); +IkReal x292=((1.0)*cj4); +IkReal x293=((1.0)*x291); +evalcond[0]=(((new_r02*x290))+new_r20); +evalcond[1]=(((new_r02*x291))+new_r21); +evalcond[2]=(x290+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x291*x292))+new_r01); +evalcond[4]=((((-1.0)*x290*x292))+new_r00); +evalcond[5]=((((-1.0)*x293))+(((-1.0)*new_r10))); +evalcond[6]=((((-1.0)*x290))+((new_r20*sj4))+((cj4*new_r00))); +evalcond[7]=(((cj4*new_r01))+(((-1.0)*x293))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x294=IKPowWithIntegerCheck(new_r02,-1); +if(!x294.valid){ +continue; +} +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21*(x294.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21*(x294.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21*(x294.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x295=IKsin(j5); +IkReal x296=IKcos(j5); +IkReal x297=((1.0)*cj4); +IkReal x298=((1.0)*x296); +evalcond[0]=(((new_r02*x295))+new_r20); +evalcond[1]=(((new_r02*x296))+new_r21); +evalcond[2]=(x295+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x296*x297))+new_r01); +evalcond[4]=((((-1.0)*x295*x297))+new_r00); +evalcond[5]=((((-1.0)*x298))+(((-1.0)*new_r10))); +evalcond[6]=((((-1.0)*x295))+((new_r20*sj4))+((cj4*new_r00))); +evalcond[7]=(((cj4*new_r01))+(((-1.0)*x298))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x299=IKPowWithIntegerCheck(IKsign(new_r02),-1); +if(!x299.valid){ +continue; +} +CheckValue x300 = IKatan2WithCheck(IkReal(((-1.0)*new_r20)),IkReal(((-1.0)*new_r21)),IKFAST_ATAN2_MAGTHRESH); +if(!x300.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x299.value)))+(x300.value)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x301=IKsin(j5); +IkReal x302=IKcos(j5); +IkReal x303=((1.0)*cj4); +IkReal x304=((1.0)*x302); +evalcond[0]=(((new_r02*x301))+new_r20); +evalcond[1]=(((new_r02*x302))+new_r21); +evalcond[2]=(x301+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x302*x303))+new_r01); +evalcond[4]=((((-1.0)*x301*x303))+new_r00); +evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x304))); +evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x301))); +evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x304))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[1]; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +j5eval[0]=1.0; +if( IKabs(j5eval[0]) < 0.0000000100000000 ) +{ +continue; // no branches [j5] + +} 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 j5array[2], cj5array[2], sj5array[2], tempj5array[1]; +int numsolutions = 0; +for(int ij5 = 0; ij5 < numroots; ++ij5) +{ +IkReal htj5 = zeror[ij5]; +tempj5array[0]=((2.0)*(atan(htj5))); +for(int kj5 = 0; kj5 < 1; ++kj5) +{ +j5array[numsolutions] = tempj5array[kj5]; +if( j5array[numsolutions] > IKPI ) +{ + j5array[numsolutions]-=IK2PI; +} +else if( j5array[numsolutions] < -IKPI ) +{ + j5array[numsolutions]+=IK2PI; +} +sj5array[numsolutions] = IKsin(j5array[numsolutions]); +cj5array[numsolutions] = IKcos(j5array[numsolutions]); +numsolutions++; +} +} +bool j5valid[2]={true,true}; +_nj5 = 2; +for(int ij5 = 0; ij5 < numsolutions; ++ij5) + { +if( !j5valid[ij5] ) +{ + continue; +} + j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +htj5 = IKtan(j5/2); + +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} + } + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j5] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x306=IKPowWithIntegerCheck(sj4,-1); +if(!x306.valid){ +continue; +} +IkReal x305=x306.value; +CheckValue x307=IKPowWithIntegerCheck(cj4,-1); +if(!x307.valid){ +continue; +} +CheckValue x308=IKPowWithIntegerCheck(sj3,-1); +if(!x308.valid){ +continue; +} +if( IKabs((new_r20*x305)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x305*(x307.value)*(x308.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x305))+IKsqr((x305*(x307.value)*(x308.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2((new_r20*x305), (x305*(x307.value)*(x308.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20)))))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[12]; +IkReal x309=IKsin(j5); +IkReal x310=IKcos(j5); +IkReal x311=(cj4*sj3); +IkReal x312=(cj3*new_r10); +IkReal x313=((1.0)*new_r01); +IkReal x314=(cj3*new_r11); +IkReal x315=((1.0)*new_r00); +IkReal x316=((1.0)*x310); +IkReal x317=(cj4*x309); +IkReal x318=((1.0)*x309); +evalcond[0]=((((-1.0)*sj4*x318))+new_r20); +evalcond[1]=((((-1.0)*sj4*x316))+new_r21); +evalcond[2]=(((new_r11*sj3))+x309+((cj3*new_r01))); +evalcond[3]=(((new_r10*sj3))+(((-1.0)*x316))+((cj3*new_r00))); +evalcond[4]=(((x310*x311))+((cj3*x309))+new_r01); +evalcond[5]=(((x309*x311))+new_r00+(((-1.0)*cj3*x316))); +evalcond[6]=(((sj3*x309))+(((-1.0)*cj3*cj4*x316))+new_r11); +evalcond[7]=((((-1.0)*sj3*x315))+x312+(((-1.0)*x317))); +evalcond[8]=((((-1.0)*sj3*x313))+(((-1.0)*cj4*x316))+x314); +evalcond[9]=((((-1.0)*cj3*x317))+(((-1.0)*sj3*x316))+new_r10); +evalcond[10]=(((new_r20*sj4))+((cj4*x312))+(((-1.0)*x318))+(((-1.0)*x311*x315))); +evalcond[11]=(((cj4*x314))+(((-1.0)*x316))+((new_r21*sj4))+(((-1.0)*x311*x313))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x320=IKPowWithIntegerCheck(sj4,-1); +if(!x320.valid){ +continue; +} +IkReal x319=x320.value; +CheckValue x321=IKPowWithIntegerCheck(cj3,-1); +if(!x321.valid){ +continue; +} +if( IKabs((new_r20*x319)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x319*(x321.value)*((((new_r00*sj4))+((cj4*new_r20*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x319))+IKsqr((x319*(x321.value)*((((new_r00*sj4))+((cj4*new_r20*sj3))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2((new_r20*x319), (x319*(x321.value)*((((new_r00*sj4))+((cj4*new_r20*sj3)))))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[12]; +IkReal x322=IKsin(j5); +IkReal x323=IKcos(j5); +IkReal x324=(cj4*sj3); +IkReal x325=(cj3*new_r10); +IkReal x326=((1.0)*new_r01); +IkReal x327=(cj3*new_r11); +IkReal x328=((1.0)*new_r00); +IkReal x329=((1.0)*x323); +IkReal x330=(cj4*x322); +IkReal x331=((1.0)*x322); +evalcond[0]=((((-1.0)*sj4*x331))+new_r20); +evalcond[1]=(new_r21+(((-1.0)*sj4*x329))); +evalcond[2]=(((new_r11*sj3))+x322+((cj3*new_r01))); +evalcond[3]=(((new_r10*sj3))+(((-1.0)*x329))+((cj3*new_r00))); +evalcond[4]=(((x323*x324))+new_r01+((cj3*x322))); +evalcond[5]=(((x322*x324))+(((-1.0)*cj3*x329))+new_r00); +evalcond[6]=(((sj3*x322))+new_r11+(((-1.0)*cj3*cj4*x329))); +evalcond[7]=(x325+(((-1.0)*x330))+(((-1.0)*sj3*x328))); +evalcond[8]=(x327+(((-1.0)*cj4*x329))+(((-1.0)*sj3*x326))); +evalcond[9]=((((-1.0)*cj3*x330))+new_r10+(((-1.0)*sj3*x329))); +evalcond[10]=(((new_r20*sj4))+((cj4*x325))+(((-1.0)*x331))+(((-1.0)*x324*x328))); +evalcond[11]=((((-1.0)*x329))+((cj4*x327))+((new_r21*sj4))+(((-1.0)*x324*x326))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x332=IKPowWithIntegerCheck(IKsign(sj4),-1); +if(!x332.valid){ +continue; +} +CheckValue x333 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH); +if(!x333.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x332.value)))+(x333.value)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[12]; +IkReal x334=IKsin(j5); +IkReal x335=IKcos(j5); +IkReal x336=(cj4*sj3); +IkReal x337=(cj3*new_r10); +IkReal x338=((1.0)*new_r01); +IkReal x339=(cj3*new_r11); +IkReal x340=((1.0)*new_r00); +IkReal x341=((1.0)*x335); +IkReal x342=(cj4*x334); +IkReal x343=((1.0)*x334); +evalcond[0]=((((-1.0)*sj4*x343))+new_r20); +evalcond[1]=((((-1.0)*sj4*x341))+new_r21); +evalcond[2]=(((new_r11*sj3))+x334+((cj3*new_r01))); +evalcond[3]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x341))); +evalcond[4]=(((x335*x336))+((cj3*x334))+new_r01); +evalcond[5]=((((-1.0)*cj3*x341))+((x334*x336))+new_r00); +evalcond[6]=(((sj3*x334))+(((-1.0)*cj3*cj4*x341))+new_r11); +evalcond[7]=(x337+(((-1.0)*sj3*x340))+(((-1.0)*x342))); +evalcond[8]=((((-1.0)*sj3*x338))+(((-1.0)*cj4*x341))+x339); +evalcond[9]=((((-1.0)*cj3*x342))+new_r10+(((-1.0)*sj3*x341))); +evalcond[10]=(((new_r20*sj4))+(((-1.0)*x336*x340))+((cj4*x337))+(((-1.0)*x343))); +evalcond[11]=(((cj4*x339))+((new_r21*sj4))+(((-1.0)*x341))+(((-1.0)*x336*x338))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x344=IKPowWithIntegerCheck(IKsign(sj4),-1); +if(!x344.valid){ +continue; +} +CheckValue x345 = IKatan2WithCheck(IkReal(new_r02),IkReal(((-1.0)*new_r12)),IKFAST_ATAN2_MAGTHRESH); +if(!x345.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x344.value)))+(x345.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x346=IKsin(j3); +IkReal x347=IKcos(j3); +IkReal x348=((1.0)*cj4); +IkReal x349=((1.0)*sj4); +IkReal x350=(new_r02*x346); +IkReal x351=(new_r12*x347); +IkReal x352=(sj4*x347); +evalcond[0]=(x352+new_r12); +evalcond[1]=((((-1.0)*x346*x349))+new_r02); +evalcond[2]=(((new_r02*x347))+((new_r12*x346))); +evalcond[3]=(sj4+(((-1.0)*x350))+x351); +evalcond[4]=((((-1.0)*x348*x350))+((cj4*x351))+((new_r22*sj4))); +evalcond[5]=((((-1.0)*new_r00*x346*x349))+(((-1.0)*new_r20*x348))+((new_r10*x352))); +evalcond[6]=((((-1.0)*new_r01*x346*x349))+(((-1.0)*new_r21*x348))+((new_r11*x352))); +evalcond[7]=((1.0)+((sj4*x351))+(((-1.0)*x349*x350))+(((-1.0)*new_r22*x348))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j5eval[3]; +j5eval[0]=sj4; +j5eval[1]=IKsign(sj4); +j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[2]; +j5eval[0]=sj4; +j5eval[1]=cj3; +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[3]; +j5eval[0]=sj4; +j5eval[1]=cj4; +j5eval[2]=sj3; +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[5]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x353=((1.0)*new_r01); +if( IKabs(((((-1.0)*cj3*x353))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r00))+(((-1.0)*sj3*x353)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x353))+(((-1.0)*new_r00*sj3))))+IKsqr((((cj3*new_r00))+(((-1.0)*sj3*x353))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*cj3*x353))+(((-1.0)*new_r00*sj3))), (((cj3*new_r00))+(((-1.0)*sj3*x353)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x354=IKsin(j5); +IkReal x355=IKcos(j5); +IkReal x356=((1.0)*sj3); +IkReal x357=(sj3*x354); +IkReal x358=((1.0)*x355); +IkReal x359=((1.0)*x354); +IkReal x360=(cj3*x358); +evalcond[0]=(((new_r11*sj3))+x354+((cj3*new_r01))); +evalcond[1]=(((new_r10*sj3))+(((-1.0)*x358))+((cj3*new_r00))); +evalcond[2]=(((cj3*x354))+((sj3*x355))+new_r01); +evalcond[3]=((((-1.0)*x360))+x357+new_r00); +evalcond[4]=((((-1.0)*x360))+x357+new_r11); +evalcond[5]=((((-1.0)*x359))+((cj3*new_r10))+(((-1.0)*new_r00*x356))); +evalcond[6]=((((-1.0)*x358))+(((-1.0)*new_r01*x356))+((cj3*new_r11))); +evalcond[7]=((((-1.0)*cj3*x359))+new_r10+(((-1.0)*x355*x356))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +IkReal x361=((1.0)*cj3); +if( IKabs(((((-1.0)*new_r11*sj3))+(((-1.0)*new_r01*x361)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((new_r01*sj3))+(((-1.0)*new_r11*x361)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*new_r11*sj3))+(((-1.0)*new_r01*x361))))+IKsqr((((new_r01*sj3))+(((-1.0)*new_r11*x361))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((((-1.0)*new_r11*sj3))+(((-1.0)*new_r01*x361))), (((new_r01*sj3))+(((-1.0)*new_r11*x361)))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x362=IKsin(j5); +IkReal x363=IKcos(j5); +IkReal x364=((1.0)*sj3); +IkReal x365=(cj3*x362); +IkReal x366=((1.0)*x363); +IkReal x367=(x363*x364); +evalcond[0]=(((new_r11*sj3))+x362+((cj3*new_r01))); +evalcond[1]=((((-1.0)*new_r00*x364))+x362+((cj3*new_r10))); +evalcond[2]=(x363+((cj3*new_r11))+(((-1.0)*new_r01*x364))); +evalcond[3]=(((new_r10*sj3))+(((-1.0)*x366))+((cj3*new_r00))); +evalcond[4]=(((sj3*x362))+new_r11+((cj3*x363))); +evalcond[5]=((((-1.0)*x367))+x365+new_r01); +evalcond[6]=((((-1.0)*x367))+x365+new_r10); +evalcond[7]=((((-1.0)*x362*x364))+new_r00+(((-1.0)*cj3*x366))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r20, new_r21); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x368=IKsin(j5); +IkReal x369=IKcos(j5); +IkReal x370=((1.0)*x369); +IkReal x371=((1.0)*x368); +evalcond[0]=((((-1.0)*x371))+new_r20); +evalcond[1]=((((-1.0)*x370))+new_r21); +evalcond[2]=(((sj3*x368))+new_r11); +evalcond[3]=((((-1.0)*new_r12*x371))+new_r01); +evalcond[4]=((((-1.0)*cj3*x370))+new_r00); +evalcond[5]=((((-1.0)*sj3*x370))+new_r10); +evalcond[6]=(((new_r11*sj3))+x368+((cj3*new_r01))); +evalcond[7]=(((new_r10*sj3))+(((-1.0)*x370))+((cj3*new_r00))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r20)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20))+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r20), ((-1.0)*new_r21)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x372=IKsin(j5); +IkReal x373=IKcos(j5); +IkReal x374=((1.0)*x373); +evalcond[0]=(x372+new_r20); +evalcond[1]=(x373+new_r21); +evalcond[2]=(((new_r12*x372))+new_r01); +evalcond[3]=(((sj3*x372))+new_r11); +evalcond[4]=((((-1.0)*cj3*x374))+new_r00); +evalcond[5]=((((-1.0)*sj3*x374))+new_r10); +evalcond[6]=(((new_r11*sj3))+x372+((cj3*new_r01))); +evalcond[7]=(((new_r10*sj3))+(((-1.0)*x374))+((cj3*new_r00))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959))); +evalcond[1]=new_r02; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r01), new_r00); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x375=IKsin(j5); +IkReal x376=IKcos(j5); +IkReal x377=((1.0)*cj4); +IkReal x378=((1.0)*x376); +IkReal x379=((1.0)*x375); +evalcond[0]=(x375+new_r01); +evalcond[1]=((((-1.0)*x378))+new_r00); +evalcond[2]=((((-1.0)*sj4*x379))+new_r20); +evalcond[3]=((((-1.0)*sj4*x378))+new_r21); +evalcond[4]=((((-1.0)*x376*x377))+new_r11); +evalcond[5]=(new_r10+(((-1.0)*x375*x377))); +evalcond[6]=(((new_r20*sj4))+((cj4*new_r10))+(((-1.0)*x379))); +evalcond[7]=(((cj4*new_r11))+(((-1.0)*x378))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +evalcond[1]=new_r02; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r01, ((-1.0)*new_r00)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x380=IKsin(j5); +IkReal x381=IKcos(j5); +IkReal x382=((1.0)*cj4); +IkReal x383=((1.0)*x381); +IkReal x384=((1.0)*x380); +evalcond[0]=(x380+(((-1.0)*new_r01))); +evalcond[1]=(new_r11+((cj4*x381))); +evalcond[2]=((((-1.0)*sj4*x384))+new_r20); +evalcond[3]=((((-1.0)*sj4*x383))+new_r21); +evalcond[4]=((((-1.0)*x383))+(((-1.0)*new_r00))); +evalcond[5]=((((-1.0)*x380*x382))+(((-1.0)*new_r10))); +evalcond[6]=((((-1.0)*new_r10*x382))+((new_r20*sj4))+(((-1.0)*x384))); +evalcond[7]=((((-1.0)*new_r11*x382))+(((-1.0)*x383))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +evalcond[1]=new_r12; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(new_r10)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r11), new_r10); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x385=IKcos(j5); +IkReal x386=IKsin(j5); +IkReal x387=((1.0)*cj4); +IkReal x388=((1.0)*x385); +IkReal x389=((1.0)*x386); +evalcond[0]=(x386+new_r11); +evalcond[1]=((((-1.0)*x388))+new_r10); +evalcond[2]=(new_r01+((cj4*x385))); +evalcond[3]=(new_r00+((cj4*x386))); +evalcond[4]=((((-1.0)*new_r02*x389))+new_r20); +evalcond[5]=((((-1.0)*new_r02*x388))+new_r21); +evalcond[6]=(((new_r20*sj4))+(((-1.0)*new_r00*x387))+(((-1.0)*x389))); +evalcond[7]=((((-1.0)*new_r01*x387))+(((-1.0)*x388))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j3)))), 6.28318530717959))); +evalcond[1]=new_r12; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[3]; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +j5eval[0]=new_r02; +j5eval[1]=IKsign(new_r02); +j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[1]; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +j5eval[0]=new_r02; +if( IKabs(j5eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j5eval[2]; +sj3=-1.0; +cj3=0; +j3=-1.5707963267949; +j5eval[0]=new_r02; +j5eval[1]=cj4; +if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[4]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +evalcond[2]=new_r01; +evalcond[3]=new_r00; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r20)+IKsqr(new_r21)-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r20, new_r21); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[4]; +IkReal x390=IKsin(j5); +IkReal x391=((1.0)*(IKcos(j5))); +evalcond[0]=((((-1.0)*x390))+new_r20); +evalcond[1]=((((-1.0)*x391))+new_r21); +evalcond[2]=(x390+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x391))+(((-1.0)*new_r10))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r22; +evalcond[2]=new_r01; +evalcond[3]=new_r00; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[4]; +IkReal x392=IKsin(j5); +IkReal x393=IKcos(j5); +evalcond[0]=(x392+new_r20); +evalcond[1]=(x393+new_r21); +evalcond[2]=(x392+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x393))+(((-1.0)*new_r10))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=IKabs(new_r02); +evalcond[1]=new_r20; +evalcond[2]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[6]; +IkReal x394=IKsin(j5); +IkReal x395=IKcos(j5); +IkReal x396=((1.0)*cj4); +IkReal x397=((1.0)*x395); +evalcond[0]=(x394+(((-1.0)*new_r11))); +evalcond[1]=((((-1.0)*x395*x396))+new_r01); +evalcond[2]=((((-1.0)*x394*x396))+new_r00); +evalcond[3]=((((-1.0)*x397))+(((-1.0)*new_r10))); +evalcond[4]=((((-1.0)*x394))+((cj4*new_r00))); +evalcond[5]=((((-1.0)*x397))+((cj4*new_r01))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r10))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r10)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[6]; +IkReal x398=IKcos(j5); +IkReal x399=IKsin(j5); +IkReal x400=((-1.0)*x399); +IkReal x401=((-1.0)*x398); +evalcond[0]=x400; +evalcond[1]=x401; +evalcond[2]=(new_r22*x401); +evalcond[3]=(new_r22*x400); +evalcond[4]=(x399+(((-1.0)*new_r11))); +evalcond[5]=((((-1.0)*x398))+(((-1.0)*new_r10))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j5] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x402=IKPowWithIntegerCheck(new_r02,-1); +if(!x402.valid){ +continue; +} +CheckValue x403=IKPowWithIntegerCheck(cj4,-1); +if(!x403.valid){ +continue; +} +if( IKabs(((-1.0)*new_r20*(x402.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x403.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r20*(x402.value)))+IKsqr((new_r01*(x403.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(((-1.0)*new_r20*(x402.value)), (new_r01*(x403.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x404=IKsin(j5); +IkReal x405=IKcos(j5); +IkReal x406=((1.0)*cj4); +IkReal x407=((1.0)*x405); +evalcond[0]=(new_r20+((new_r02*x404))); +evalcond[1]=(new_r21+((new_r02*x405))); +evalcond[2]=(x404+(((-1.0)*new_r11))); +evalcond[3]=(new_r01+(((-1.0)*x405*x406))); +evalcond[4]=((((-1.0)*x404*x406))+new_r00); +evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x407))); +evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x404))); +evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x407))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x408=IKPowWithIntegerCheck(new_r02,-1); +if(!x408.valid){ +continue; +} +if( IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r21*(x408.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r11)+IKsqr(((-1.0)*new_r21*(x408.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2(new_r11, ((-1.0)*new_r21*(x408.value))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x409=IKsin(j5); +IkReal x410=IKcos(j5); +IkReal x411=((1.0)*cj4); +IkReal x412=((1.0)*x410); +evalcond[0]=(new_r20+((new_r02*x409))); +evalcond[1]=(new_r21+((new_r02*x410))); +evalcond[2]=(x409+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x410*x411))+new_r01); +evalcond[4]=(new_r00+(((-1.0)*x409*x411))); +evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x412))); +evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x409))); +evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x412))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x413=IKPowWithIntegerCheck(IKsign(new_r02),-1); +if(!x413.valid){ +continue; +} +CheckValue x414 = IKatan2WithCheck(IkReal(((-1.0)*new_r20)),IkReal(((-1.0)*new_r21)),IKFAST_ATAN2_MAGTHRESH); +if(!x414.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x413.value)))+(x414.value)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[8]; +IkReal x415=IKsin(j5); +IkReal x416=IKcos(j5); +IkReal x417=((1.0)*cj4); +IkReal x418=((1.0)*x416); +evalcond[0]=(new_r20+((new_r02*x415))); +evalcond[1]=(new_r21+((new_r02*x416))); +evalcond[2]=(x415+(((-1.0)*new_r11))); +evalcond[3]=((((-1.0)*x416*x417))+new_r01); +evalcond[4]=((((-1.0)*x415*x417))+new_r00); +evalcond[5]=((((-1.0)*new_r10))+(((-1.0)*x418))); +evalcond[6]=(((new_r20*sj4))+((cj4*new_r00))+(((-1.0)*x415))); +evalcond[7]=(((cj4*new_r01))+((new_r21*sj4))+(((-1.0)*x418))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j5eval[1]; +new_r21=0; +new_r20=0; +new_r02=0; +new_r12=0; +j5eval[0]=1.0; +if( IKabs(j5eval[0]) < 0.0000000100000000 ) +{ +continue; // no branches [j5] + +} 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 j5array[2], cj5array[2], sj5array[2], tempj5array[1]; +int numsolutions = 0; +for(int ij5 = 0; ij5 < numroots; ++ij5) +{ +IkReal htj5 = zeror[ij5]; +tempj5array[0]=((2.0)*(atan(htj5))); +for(int kj5 = 0; kj5 < 1; ++kj5) +{ +j5array[numsolutions] = tempj5array[kj5]; +if( j5array[numsolutions] > IKPI ) +{ + j5array[numsolutions]-=IK2PI; +} +else if( j5array[numsolutions] < -IKPI ) +{ + j5array[numsolutions]+=IK2PI; +} +sj5array[numsolutions] = IKsin(j5array[numsolutions]); +cj5array[numsolutions] = IKcos(j5array[numsolutions]); +numsolutions++; +} +} +bool j5valid[2]={true,true}; +_nj5 = 2; +for(int ij5 = 0; ij5 < numsolutions; ++ij5) + { +if( !j5valid[ij5] ) +{ + continue; +} + j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +htj5 = IKtan(j5/2); + +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} + } + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j5] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x420=IKPowWithIntegerCheck(sj4,-1); +if(!x420.valid){ +continue; +} +IkReal x419=x420.value; +CheckValue x421=IKPowWithIntegerCheck(cj4,-1); +if(!x421.valid){ +continue; +} +CheckValue x422=IKPowWithIntegerCheck(sj3,-1); +if(!x422.valid){ +continue; +} +if( IKabs((new_r20*x419)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x419*(x421.value)*(x422.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x419))+IKsqr((x419*(x421.value)*(x422.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2((new_r20*x419), (x419*(x421.value)*(x422.value)*(((((-1.0)*new_r01*sj4))+(((-1.0)*cj3*new_r20)))))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[12]; +IkReal x423=IKsin(j5); +IkReal x424=IKcos(j5); +IkReal x425=(cj4*sj3); +IkReal x426=(cj3*new_r10); +IkReal x427=((1.0)*new_r01); +IkReal x428=(cj3*new_r11); +IkReal x429=((1.0)*new_r00); +IkReal x430=((1.0)*x424); +IkReal x431=(cj4*x423); +IkReal x432=((1.0)*x423); +evalcond[0]=(new_r20+(((-1.0)*sj4*x432))); +evalcond[1]=(new_r21+(((-1.0)*sj4*x430))); +evalcond[2]=(((new_r11*sj3))+x423+((cj3*new_r01))); +evalcond[3]=((((-1.0)*x430))+((new_r10*sj3))+((cj3*new_r00))); +evalcond[4]=(((x424*x425))+((cj3*x423))+new_r01); +evalcond[5]=((((-1.0)*cj3*x430))+((x423*x425))+new_r00); +evalcond[6]=((((-1.0)*cj3*cj4*x430))+((sj3*x423))+new_r11); +evalcond[7]=((((-1.0)*sj3*x429))+(((-1.0)*x431))+x426); +evalcond[8]=((((-1.0)*sj3*x427))+(((-1.0)*cj4*x430))+x428); +evalcond[9]=((((-1.0)*sj3*x430))+(((-1.0)*cj3*x431))+new_r10); +evalcond[10]=((((-1.0)*x432))+((new_r20*sj4))+((cj4*x426))+(((-1.0)*x425*x429))); +evalcond[11]=((((-1.0)*x430))+((cj4*x428))+(((-1.0)*x425*x427))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x434=IKPowWithIntegerCheck(sj4,-1); +if(!x434.valid){ +continue; +} +IkReal x433=x434.value; +CheckValue x435=IKPowWithIntegerCheck(cj3,-1); +if(!x435.valid){ +continue; +} +if( IKabs((new_r20*x433)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x433*(x435.value)*((((new_r00*sj4))+((cj4*new_r20*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r20*x433))+IKsqr((x433*(x435.value)*((((new_r00*sj4))+((cj4*new_r20*sj3))))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j5array[0]=IKatan2((new_r20*x433), (x433*(x435.value)*((((new_r00*sj4))+((cj4*new_r20*sj3)))))); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[12]; +IkReal x436=IKsin(j5); +IkReal x437=IKcos(j5); +IkReal x438=(cj4*sj3); +IkReal x439=(cj3*new_r10); +IkReal x440=((1.0)*new_r01); +IkReal x441=(cj3*new_r11); +IkReal x442=((1.0)*new_r00); +IkReal x443=((1.0)*x437); +IkReal x444=(cj4*x436); +IkReal x445=((1.0)*x436); +evalcond[0]=((((-1.0)*sj4*x445))+new_r20); +evalcond[1]=((((-1.0)*sj4*x443))+new_r21); +evalcond[2]=(((new_r11*sj3))+x436+((cj3*new_r01))); +evalcond[3]=((((-1.0)*x443))+((new_r10*sj3))+((cj3*new_r00))); +evalcond[4]=(((cj3*x436))+new_r01+((x437*x438))); +evalcond[5]=(((x436*x438))+(((-1.0)*cj3*x443))+new_r00); +evalcond[6]=(((sj3*x436))+(((-1.0)*cj3*cj4*x443))+new_r11); +evalcond[7]=((((-1.0)*sj3*x442))+(((-1.0)*x444))+x439); +evalcond[8]=((((-1.0)*sj3*x440))+(((-1.0)*cj4*x443))+x441); +evalcond[9]=((((-1.0)*sj3*x443))+(((-1.0)*cj3*x444))+new_r10); +evalcond[10]=((((-1.0)*x445))+(((-1.0)*x438*x442))+((new_r20*sj4))+((cj4*x439))); +evalcond[11]=((((-1.0)*x443))+(((-1.0)*x438*x440))+((cj4*x441))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x446=IKPowWithIntegerCheck(IKsign(sj4),-1); +if(!x446.valid){ +continue; +} +CheckValue x447 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH); +if(!x447.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x446.value)))+(x447.value)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[12]; +IkReal x448=IKsin(j5); +IkReal x449=IKcos(j5); +IkReal x450=(cj4*sj3); +IkReal x451=(cj3*new_r10); +IkReal x452=((1.0)*new_r01); +IkReal x453=(cj3*new_r11); +IkReal x454=((1.0)*new_r00); +IkReal x455=((1.0)*x449); +IkReal x456=(cj4*x448); +IkReal x457=((1.0)*x448); +evalcond[0]=((((-1.0)*sj4*x457))+new_r20); +evalcond[1]=((((-1.0)*sj4*x455))+new_r21); +evalcond[2]=(((new_r11*sj3))+x448+((cj3*new_r01))); +evalcond[3]=(((new_r10*sj3))+(((-1.0)*x455))+((cj3*new_r00))); +evalcond[4]=(((x449*x450))+((cj3*x448))+new_r01); +evalcond[5]=(((x448*x450))+new_r00+(((-1.0)*cj3*x455))); +evalcond[6]=((((-1.0)*cj3*cj4*x455))+((sj3*x448))+new_r11); +evalcond[7]=((((-1.0)*x456))+(((-1.0)*sj3*x454))+x451); +evalcond[8]=((((-1.0)*cj4*x455))+(((-1.0)*sj3*x452))+x453); +evalcond[9]=((((-1.0)*sj3*x455))+new_r10+(((-1.0)*cj3*x456))); +evalcond[10]=(((new_r20*sj4))+((cj4*x451))+(((-1.0)*x457))+(((-1.0)*x450*x454))); +evalcond[11]=(((cj4*x453))+(((-1.0)*x455))+(((-1.0)*x450*x452))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} + +} else +{ +{ +IkReal j5array[1], cj5array[1], sj5array[1]; +bool j5valid[1]={false}; +_nj5 = 1; +CheckValue x458=IKPowWithIntegerCheck(IKsign(sj4),-1); +if(!x458.valid){ +continue; +} +CheckValue x459 = IKatan2WithCheck(IkReal(new_r20),IkReal(new_r21),IKFAST_ATAN2_MAGTHRESH); +if(!x459.valid){ +continue; +} +j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x458.value)))+(x459.value)); +sj5array[0]=IKsin(j5array[0]); +cj5array[0]=IKcos(j5array[0]); +if( j5array[0] > IKPI ) +{ + j5array[0]-=IK2PI; +} +else if( j5array[0] < -IKPI ) +{ j5array[0]+=IK2PI; +} +j5valid[0] = true; +for(int ij5 = 0; ij5 < 1; ++ij5) +{ +if( !j5valid[ij5] ) +{ + continue; +} +_ij5[0] = ij5; _ij5[1] = -1; +for(int iij5 = ij5+1; iij5 < 1; ++iij5) +{ +if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH ) +{ + j5valid[iij5]=false; _ij5[1] = iij5; break; +} +} +j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5]; +{ +IkReal evalcond[2]; +IkReal x460=((1.0)*sj4); +evalcond[0]=((((-1.0)*x460*(IKsin(j5))))+new_r20); +evalcond[1]=((((-1.0)*x460*(IKcos(j5))))+new_r21); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +IkReal j3eval[3]; +j3eval[0]=sj4; +j3eval[1]=IKsign(sj4); +j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[2]; +j3eval[0]=new_r11; +j3eval[1]=sj4; +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[5]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x461=((1.0)*new_r01); +if( IKabs(((((-1.0)*cj5*x461))+(((-1.0)*new_r00*sj5)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj5*x461))+((cj5*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj5*x461))+(((-1.0)*new_r00*sj5))))+IKsqr(((((-1.0)*sj5*x461))+((cj5*new_r00))))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((((-1.0)*cj5*x461))+(((-1.0)*new_r00*sj5))), ((((-1.0)*sj5*x461))+((cj5*new_r00)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x462=IKcos(j3); +IkReal x463=IKsin(j3); +IkReal x464=((1.0)*cj5); +IkReal x465=(sj5*x463); +IkReal x466=(sj5*x462); +IkReal x467=((1.0)*x463); +IkReal x468=(x462*x464); +evalcond[0]=(((new_r11*x463))+sj5+((new_r01*x462))); +evalcond[1]=(((cj5*x463))+x466+new_r01); +evalcond[2]=((((-1.0)*x468))+x465+new_r00); +evalcond[3]=((((-1.0)*x468))+x465+new_r11); +evalcond[4]=(((new_r10*x463))+((new_r00*x462))+(((-1.0)*x464))); +evalcond[5]=((((-1.0)*x463*x464))+(((-1.0)*x466))+new_r10); +evalcond[6]=((((-1.0)*sj5))+((new_r10*x462))+(((-1.0)*new_r00*x467))); +evalcond[7]=(((new_r11*x462))+(((-1.0)*new_r01*x467))+(((-1.0)*x464))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j4)))), 6.28318530717959))); +evalcond[1]=new_r12; +evalcond[2]=new_r02; +evalcond[3]=new_r20; +evalcond[4]=new_r21; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[3]; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +IkReal x469=(((new_r11*sj5))+((cj5*new_r01))); +j3eval[0]=x469; +j3eval[1]=IKsign(x469); +j3eval[2]=((IKabs(((((-1.0)*cj5*sj5))+(((-1.0)*new_r01*new_r11)))))+(IKabs(((-1.0)+(new_r01*new_r01)+(cj5*cj5))))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +IkReal x470=((1.0)*sj5); +IkReal x471=(((new_r10*new_r11))+((new_r00*new_r01))); +j3eval[0]=x471; +j3eval[1]=IKsign(x471); +j3eval[2]=((IKabs(((((-1.0)*new_r00*x470))+(((-1.0)*new_r11*x470)))))+(IKabs((((new_r01*sj5))+(((-1.0)*new_r10*x470)))))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +IkReal x472=((1.0)*sj5); +IkReal x473=((new_r01*new_r01)+(new_r11*new_r11)); +j3eval[0]=x473; +j3eval[1]=((IKabs((((cj5*new_r01))+(((-1.0)*new_r11*x472)))))+(IKabs(((((-1.0)*new_r01*x472))+(((-1.0)*cj5*new_r11)))))); +j3eval[2]=IKsign(x473); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[1]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((new_r01*new_r01)+(new_r11*new_r11)); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +new_r01=0; +new_r11=0; +j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x475 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH); +if(!x475.valid){ +continue; +} +IkReal x474=x475.value; +j3array[0]=((-1.0)*x474); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((3.14159265358979)+(((-1.0)*x474))); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*new_r00*(IKsin(j3))))+((new_r10*(IKcos(j3))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +if( IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r01)+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(new_r01, ((-1.0)*new_r11)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x476=IKcos(j3); +IkReal x477=IKsin(j3); +IkReal x478=((1.0)*x477); +evalcond[0]=(x476+new_r11); +evalcond[1]=((((-1.0)*x478))+new_r01); +evalcond[2]=((((-1.0)*x476))+new_r00); +evalcond[3]=((((-1.0)*x478))+new_r10); +evalcond[4]=(((new_r01*x476))+((new_r11*x477))); +evalcond[5]=((-1.0)+((new_r10*x477))+((new_r00*x476))); +evalcond[6]=(((new_r10*x476))+(((-1.0)*new_r00*x478))); +evalcond[7]=((1.0)+((new_r11*x476))+(((-1.0)*new_r01*x478))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +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)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x479=IKsin(j3); +IkReal x480=IKcos(j3); +IkReal x481=((1.0)*x479); +evalcond[0]=(x479+new_r01); +evalcond[1]=(x480+new_r00); +evalcond[2]=(x479+new_r10); +evalcond[3]=((((-1.0)*x480))+new_r11); +evalcond[4]=(((new_r11*x479))+((new_r01*x480))); +evalcond[5]=((1.0)+((new_r10*x479))+((new_r00*x480))); +evalcond[6]=((((-1.0)*new_r00*x481))+((new_r10*x480))); +evalcond[7]=((-1.0)+(((-1.0)*new_r01*x481))+((new_r11*x480))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +IkReal x483 = ((new_r01*new_r01)+(new_r11*new_r11)); +if(IKabs(x483)==0){ +continue; +} +IkReal x482=pow(x483,-0.5); +CheckValue x484 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x484.valid){ +continue; +} +IkReal gconst6=((-1.0)*(x484.value)); +IkReal gconst7=((-1.0)*new_r01*x482); +IkReal gconst8=(new_r11*x482); +CheckValue x485 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x485.valid){ +continue; +} +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x485.value)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[3]; +CheckValue x488 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x488.valid){ +continue; +} +IkReal x486=((-1.0)*(x488.value)); +IkReal x487=x482; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x486; +IkReal gconst6=x486; +IkReal gconst7=((-1.0)*new_r01*x487); +IkReal gconst8=(new_r11*x487); +IkReal x489=new_r01*new_r01; +IkReal x490=(new_r00*new_r01); +IkReal x491=(((new_r10*new_r11))+x490); +IkReal x492=x482; +IkReal x493=(new_r01*x492); +j3eval[0]=x491; +j3eval[1]=IKsign(x491); +j3eval[2]=((IKabs(((((-1.0)*x489*x492))+((new_r10*x493)))))+(IKabs((((x490*x492))+((new_r11*x493)))))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[2]; +CheckValue x496 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x496.valid){ +continue; +} +IkReal x494=((-1.0)*(x496.value)); +IkReal x495=x482; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x494; +IkReal gconst6=x494; +IkReal gconst7=((-1.0)*new_r01*x495); +IkReal gconst8=(new_r11*x495); +IkReal x497=((new_r01*new_r01)+(new_r11*new_r11)); +j3eval[0]=x497; +j3eval[1]=IKsign(x497); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x500 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x500.valid){ +continue; +} +IkReal x498=((-1.0)*(x500.value)); +IkReal x499=x482; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x498; +IkReal gconst6=x498; +IkReal gconst7=((-1.0)*new_r01*x499); +IkReal gconst8=(new_r11*x499); +j3eval[0]=((new_r01*new_r01)+(new_r11*new_r11)); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[5]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((gconst7*gconst7)+(gconst8*gconst8)); +evalcond[1]=new_r01; +evalcond[2]=new_r00; +evalcond[3]=new_r11; +evalcond[4]=new_r10; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[4], cj3array[4], sj3array[4]; +bool j3valid[4]={false}; +_nj3 = 4; +j3array[0]=0; +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=1.5707963267949; +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +j3array[2]=3.14159265358979; +sj3array[2]=IKsin(j3array[2]); +cj3array[2]=IKcos(j3array[2]); +j3array[3]=-1.5707963267949; +sj3array[3]=IKsin(j3array[3]); +cj3array[3]=IKcos(j3array[3]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +if( j3array[2] > IKPI ) +{ + j3array[2]-=IK2PI; +} +else if( j3array[2] < -IKPI ) +{ j3array[2]+=IK2PI; +} +j3valid[2] = true; +if( j3array[3] > IKPI ) +{ + j3array[3]-=IK2PI; +} +else if( j3array[3] < -IKPI ) +{ j3array[3]+=IK2PI; +} +j3valid[3] = true; +for(int ij3 = 0; ij3 < 4; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 4; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +CheckValue x502 = IKatan2WithCheck(IkReal(new_r01),IkReal(0),IKFAST_ATAN2_MAGTHRESH); +if(!x502.valid){ +continue; +} +IkReal x501=((-1.0)*(x502.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x501; +new_r11=0; +new_r00=0; +IkReal gconst6=x501; +IkReal x503 = new_r01*new_r01; +if(IKabs(x503)==0){ +continue; +} +IkReal gconst7=((-1.0)*new_r01*(pow(x503,-0.5))); +IkReal gconst8=0; +j3eval[0]=new_r01; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x504=IKPowWithIntegerCheck(gconst7,-1); +if(!x504.valid){ +continue; +} +cj3array[0]=((-1.0)*new_r01*(x504.value)); +if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j3valid[0] = j3valid[1] = true; + j3array[0] = IKacos(cj3array[0]); + sj3array[0] = IKsin(j3array[0]); + cj3array[1] = cj3array[0]; + j3array[1] = -j3array[0]; + sj3array[1] = -sj3array[0]; +} +else if( isnan(cj3array[0]) ) +{ + // probably any value will work + j3valid[0] = true; + cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; +} +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x505=IKsin(j3); +IkReal x506=IKcos(j3); +IkReal x507=((-1.0)*x505); +evalcond[0]=(new_r10*x505); +evalcond[1]=(gconst7*x507); +evalcond[2]=(new_r01*x507); +evalcond[3]=(gconst7+((new_r01*x506))); +evalcond[4]=(gconst7+((new_r10*x506))); +evalcond[5]=(((gconst7*x506))+new_r10); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x508=IKPowWithIntegerCheck(new_r01,-1); +if(!x508.valid){ +continue; +} +cj3array[0]=((-1.0)*gconst7*(x508.value)); +if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j3valid[0] = j3valid[1] = true; + j3array[0] = IKacos(cj3array[0]); + sj3array[0] = IKsin(j3array[0]); + cj3array[1] = cj3array[0]; + j3array[1] = -j3array[0]; + sj3array[1] = -sj3array[0]; +} +else if( isnan(cj3array[0]) ) +{ + // probably any value will work + j3valid[0] = true; + cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; +} +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x509=IKsin(j3); +IkReal x510=IKcos(j3); +IkReal x511=(gconst7*x510); +IkReal x512=((-1.0)*x509); +evalcond[0]=(new_r10*x509); +evalcond[1]=(gconst7*x512); +evalcond[2]=(new_r01*x512); +evalcond[3]=(x511+new_r01); +evalcond[4]=(((new_r10*x510))+gconst7); +evalcond[5]=(x511+new_r10); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00))); +evalcond[1]=gconst7; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[3]; +CheckValue x514 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x514.valid){ +continue; +} +IkReal x513=((-1.0)*(x514.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x513; +new_r00=0; +new_r10=0; +new_r21=0; +new_r22=0; +IkReal gconst6=x513; +IkReal gconst7=((-1.0)*new_r01); +IkReal gconst8=new_r11; +j3eval[0]=-1.0; +j3eval[1]=-1.0; +j3eval[2]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11)))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +CheckValue x516 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x516.valid){ +continue; +} +IkReal x515=((-1.0)*(x516.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x515; +new_r00=0; +new_r10=0; +new_r21=0; +new_r22=0; +IkReal gconst6=x515; +IkReal gconst7=((-1.0)*new_r01); +IkReal gconst8=new_r11; +j3eval[0]=-1.0; +j3eval[1]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))); +j3eval[2]=-1.0; +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +CheckValue x518 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x518.valid){ +continue; +} +IkReal x517=((-1.0)*(x518.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x517; +new_r00=0; +new_r10=0; +new_r21=0; +new_r22=0; +IkReal gconst6=x517; +IkReal gconst7=((-1.0)*new_r01); +IkReal gconst8=new_r11; +j3eval[0]=1.0; +j3eval[1]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11)))); +j3eval[2]=1.0; +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x519=((1.0)*new_r11); +CheckValue x520 = IKatan2WithCheck(IkReal((((gconst8*new_r01))+(((-1.0)*gconst7*x519)))),IkReal(((((-1.0)*gconst7*new_r01))+(((-1.0)*gconst8*x519)))),IKFAST_ATAN2_MAGTHRESH); +if(!x520.valid){ +continue; +} +CheckValue x521=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1); +if(!x521.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x520.value)+(((1.5707963267949)*(x521.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x522=IKcos(j3); +IkReal x523=IKsin(j3); +IkReal x524=((1.0)*gconst8); +IkReal x525=(gconst7*x522); +IkReal x526=(gconst7*x523); +IkReal x527=(x523*x524); +evalcond[0]=(((new_r01*x522))+gconst7+((new_r11*x523))); +evalcond[1]=(x526+((gconst8*x522))+new_r11); +evalcond[2]=((((-1.0)*x527))+x525); +evalcond[3]=((((-1.0)*x527))+x525+new_r01); +evalcond[4]=((((-1.0)*x526))+(((-1.0)*x522*x524))); +evalcond[5]=(gconst8+((new_r11*x522))+(((-1.0)*new_r01*x523))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x528 = IKatan2WithCheck(IkReal((gconst7*new_r11)),IkReal((gconst8*new_r11)),IKFAST_ATAN2_MAGTHRESH); +if(!x528.valid){ +continue; +} +CheckValue x529=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1); +if(!x529.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x528.value)+(((1.5707963267949)*(x529.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x530=IKcos(j3); +IkReal x531=IKsin(j3); +IkReal x532=((1.0)*gconst8); +IkReal x533=(gconst7*x530); +IkReal x534=(gconst7*x531); +IkReal x535=(x531*x532); +evalcond[0]=(((new_r01*x530))+gconst7+((new_r11*x531))); +evalcond[1]=(x534+((gconst8*x530))+new_r11); +evalcond[2]=((((-1.0)*x535))+x533); +evalcond[3]=((((-1.0)*x535))+x533+new_r01); +evalcond[4]=((((-1.0)*x534))+(((-1.0)*x530*x532))); +evalcond[5]=(gconst8+((new_r11*x530))+(((-1.0)*new_r01*x531))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x536 = IKatan2WithCheck(IkReal((gconst7*gconst8)),IkReal(((-1.0)*(gconst7*gconst7))),IKFAST_ATAN2_MAGTHRESH); +if(!x536.valid){ +continue; +} +CheckValue x537=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst8*new_r11))+((gconst7*new_r01)))),-1); +if(!x537.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x536.value)+(((1.5707963267949)*(x537.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x538=IKcos(j3); +IkReal x539=IKsin(j3); +IkReal x540=((1.0)*gconst8); +IkReal x541=(gconst7*x538); +IkReal x542=(gconst7*x539); +IkReal x543=(x539*x540); +evalcond[0]=(((new_r01*x538))+gconst7+((new_r11*x539))); +evalcond[1]=(x542+((gconst8*x538))+new_r11); +evalcond[2]=((((-1.0)*x543))+x541); +evalcond[3]=((((-1.0)*x543))+x541+new_r01); +evalcond[4]=((((-1.0)*x542))+(((-1.0)*x538*x540))); +evalcond[5]=(gconst8+((new_r11*x538))+(((-1.0)*new_r01*x539))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x544=IKPowWithIntegerCheck(gconst8,-1); +if(!x544.valid){ +continue; +} +cj3array[0]=(new_r00*(x544.value)); +if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j3valid[0] = j3valid[1] = true; + j3array[0] = IKacos(cj3array[0]); + sj3array[0] = IKsin(j3array[0]); + cj3array[1] = cj3array[0]; + j3array[1] = -j3array[0]; + sj3array[1] = -sj3array[0]; +} +else if( isnan(cj3array[0]) ) +{ + // probably any value will work + j3valid[0] = true; + cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; +} +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x545=IKsin(j3); +IkReal x546=IKcos(j3); +IkReal x547=((-1.0)*x545); +evalcond[0]=(new_r11*x545); +evalcond[1]=(gconst8*x547); +evalcond[2]=(new_r00*x547); +evalcond[3]=(((gconst8*x546))+new_r11); +evalcond[4]=(gconst8+((new_r11*x546))); +evalcond[5]=(((new_r00*x546))+(((-1.0)*gconst8))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r00))+(IKabs(new_r01))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +CheckValue x549 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x549.valid){ +continue; +} +IkReal x548=((-1.0)*(x549.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x548; +new_r00=0; +new_r01=0; +new_r12=0; +new_r22=0; +IkReal gconst6=x548; +IkReal gconst7=0; +IkReal x550 = ((1.0)+(((-1.0)*(new_r10*new_r10)))); +if(IKabs(x550)==0){ +continue; +} +IkReal gconst8=(new_r11*(pow(x550,-0.5))); +j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x552 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x552.valid){ +continue; +} +IkReal x551=((-1.0)*(x552.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x551; +new_r00=0; +new_r01=0; +new_r12=0; +new_r22=0; +IkReal gconst6=x551; +IkReal gconst7=0; +IkReal x553 = ((1.0)+(((-1.0)*(new_r10*new_r10)))); +if(IKabs(x553)==0){ +continue; +} +IkReal gconst8=(new_r11*(pow(x553,-0.5))); +j3eval[0]=new_r11; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x555 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x555.valid){ +continue; +} +IkReal x554=((-1.0)*(x555.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x554; +new_r00=0; +new_r01=0; +new_r12=0; +new_r22=0; +IkReal gconst6=x554; +IkReal gconst7=0; +IkReal x556 = ((1.0)+(((-1.0)*(new_r10*new_r10)))); +if(IKabs(x556)==0){ +continue; +} +IkReal gconst8=(new_r11*(pow(x556,-0.5))); +j3eval[0]=new_r10; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x557=IKPowWithIntegerCheck(new_r10,-1); +if(!x557.valid){ +continue; +} +CheckValue x558=IKPowWithIntegerCheck(gconst8,-1); +if(!x558.valid){ +continue; +} +if( IKabs((gconst8*(x557.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11*(x558.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((gconst8*(x557.value)))+IKsqr(((-1.0)*new_r11*(x558.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((gconst8*(x557.value)), ((-1.0)*new_r11*(x558.value))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x559=IKsin(j3); +IkReal x560=IKcos(j3); +IkReal x561=((1.0)*gconst8); +IkReal x562=((-1.0)*gconst8); +evalcond[0]=(new_r11*x559); +evalcond[1]=(new_r10*x560); +evalcond[2]=(x559*x562); +evalcond[3]=(x560*x562); +evalcond[4]=(((gconst8*x560))+new_r11); +evalcond[5]=(gconst8+((new_r11*x560))); +evalcond[6]=((((-1.0)*x559*x561))+new_r10); +evalcond[7]=(((new_r10*x559))+(((-1.0)*x561))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x563=IKPowWithIntegerCheck(gconst8,-1); +if(!x563.valid){ +continue; +} +CheckValue x564=IKPowWithIntegerCheck(new_r11,-1); +if(!x564.valid){ +continue; +} +if( IKabs((new_r10*(x563.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst8*(x564.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x563.value)))+IKsqr(((-1.0)*gconst8*(x564.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((new_r10*(x563.value)), ((-1.0)*gconst8*(x564.value))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x565=IKsin(j3); +IkReal x566=IKcos(j3); +IkReal x567=((1.0)*gconst8); +IkReal x568=((-1.0)*gconst8); +evalcond[0]=(new_r11*x565); +evalcond[1]=(new_r10*x566); +evalcond[2]=(x565*x568); +evalcond[3]=(x566*x568); +evalcond[4]=(((gconst8*x566))+new_r11); +evalcond[5]=(gconst8+((new_r11*x566))); +evalcond[6]=(new_r10+(((-1.0)*x565*x567))); +evalcond[7]=(((new_r10*x565))+(((-1.0)*x567))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x569 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH); +if(!x569.valid){ +continue; +} +CheckValue x570=IKPowWithIntegerCheck(IKsign(gconst8),-1); +if(!x570.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x569.value)+(((1.5707963267949)*(x570.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x571=IKsin(j3); +IkReal x572=IKcos(j3); +IkReal x573=((1.0)*gconst8); +IkReal x574=((-1.0)*gconst8); +evalcond[0]=(new_r11*x571); +evalcond[1]=(new_r10*x572); +evalcond[2]=(x571*x574); +evalcond[3]=(x572*x574); +evalcond[4]=(((gconst8*x572))+new_r11); +evalcond[5]=(((new_r11*x572))+gconst8); +evalcond[6]=((((-1.0)*x571*x573))+new_r10); +evalcond[7]=(((new_r10*x571))+(((-1.0)*x573))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=IKabs(new_r01); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +CheckValue x576 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x576.valid){ +continue; +} +IkReal x575=((-1.0)*(x576.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x575; +new_r01=0; +IkReal gconst6=x575; +IkReal gconst7=0; +IkReal x577 = new_r11*new_r11; +if(IKabs(x577)==0){ +continue; +} +IkReal gconst8=(new_r11*(pow(x577,-0.5))); +j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x579 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x579.valid){ +continue; +} +IkReal x578=((-1.0)*(x579.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x578; +new_r01=0; +IkReal gconst6=x578; +IkReal gconst7=0; +IkReal x580 = new_r11*new_r11; +if(IKabs(x580)==0){ +continue; +} +IkReal gconst8=(new_r11*(pow(x580,-0.5))); +j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x582 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x582.valid){ +continue; +} +IkReal x581=((-1.0)*(x582.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst7; +cj5=gconst8; +j5=x581; +new_r01=0; +IkReal gconst6=x581; +IkReal gconst7=0; +IkReal x583 = new_r11*new_r11; +if(IKabs(x583)==0){ +continue; +} +IkReal gconst8=(new_r11*(pow(x583,-0.5))); +j3eval[0]=new_r11; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x584=IKPowWithIntegerCheck(gconst8,-1); +if(!x584.valid){ +continue; +} +CheckValue x585=IKPowWithIntegerCheck(new_r11,-1); +if(!x585.valid){ +continue; +} +if( IKabs((new_r10*(x584.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst8*(x585.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x584.value)))+IKsqr(((-1.0)*gconst8*(x585.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((new_r10*(x584.value)), ((-1.0)*gconst8*(x585.value))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x586=IKsin(j3); +IkReal x587=IKcos(j3); +IkReal x588=((1.0)*gconst8); +evalcond[0]=(new_r11*x586); +evalcond[1]=((-1.0)*gconst8*x586); +evalcond[2]=(((gconst8*x587))+new_r11); +evalcond[3]=(gconst8+((new_r11*x587))); +evalcond[4]=((((-1.0)*x587*x588))+new_r00); +evalcond[5]=((((-1.0)*x586*x588))+new_r10); +evalcond[6]=((((-1.0)*new_r00*x586))+((new_r10*x587))); +evalcond[7]=(((new_r00*x587))+(((-1.0)*x588))+((new_r10*x586))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x589 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH); +if(!x589.valid){ +continue; +} +CheckValue x590=IKPowWithIntegerCheck(IKsign(gconst8),-1); +if(!x590.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x589.value)+(((1.5707963267949)*(x590.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x591=IKsin(j3); +IkReal x592=IKcos(j3); +IkReal x593=((1.0)*gconst8); +evalcond[0]=(new_r11*x591); +evalcond[1]=((-1.0)*gconst8*x591); +evalcond[2]=(((gconst8*x592))+new_r11); +evalcond[3]=(((new_r11*x592))+gconst8); +evalcond[4]=((((-1.0)*x592*x593))+new_r00); +evalcond[5]=(new_r10+(((-1.0)*x591*x593))); +evalcond[6]=(((new_r10*x592))+(((-1.0)*new_r00*x591))); +evalcond[7]=(((new_r10*x591))+((new_r00*x592))+(((-1.0)*x593))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x594=IKPowWithIntegerCheck(IKsign(gconst8),-1); +if(!x594.valid){ +continue; +} +CheckValue x595 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH); +if(!x595.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x594.value)))+(x595.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x596=IKsin(j3); +IkReal x597=IKcos(j3); +IkReal x598=((1.0)*gconst8); +evalcond[0]=(new_r11*x596); +evalcond[1]=((-1.0)*gconst8*x596); +evalcond[2]=(((gconst8*x597))+new_r11); +evalcond[3]=(((new_r11*x597))+gconst8); +evalcond[4]=((((-1.0)*x597*x598))+new_r00); +evalcond[5]=((((-1.0)*x596*x598))+new_r10); +evalcond[6]=(((new_r10*x597))+(((-1.0)*new_r00*x596))); +evalcond[7]=(((new_r10*x596))+((new_r00*x597))+(((-1.0)*x598))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j3] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x599=((1.0)*new_r11); +CheckValue x600=IKPowWithIntegerCheck(IKsign(((gconst7*gconst7)+(gconst8*gconst8))),-1); +if(!x600.valid){ +continue; +} +CheckValue x601 = IKatan2WithCheck(IkReal((((gconst8*new_r01))+(((-1.0)*gconst7*x599)))),IkReal(((((-1.0)*gconst7*new_r01))+(((-1.0)*gconst8*x599)))),IKFAST_ATAN2_MAGTHRESH); +if(!x601.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x600.value)))+(x601.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x602=IKcos(j3); +IkReal x603=IKsin(j3); +IkReal x604=((1.0)*gconst8); +IkReal x605=(gconst7*x602); +IkReal x606=(gconst7*x603); +IkReal x607=((1.0)*x603); +IkReal x608=(x603*x604); +evalcond[0]=(gconst7+((new_r11*x603))+((new_r01*x602))); +evalcond[1]=(((gconst8*x602))+x606+new_r11); +evalcond[2]=(x605+(((-1.0)*x608))+new_r01); +evalcond[3]=(gconst7+((new_r10*x602))+(((-1.0)*new_r00*x607))); +evalcond[4]=(gconst8+((new_r11*x602))+(((-1.0)*new_r01*x607))); +evalcond[5]=(x605+(((-1.0)*x608))+new_r10); +evalcond[6]=(((new_r10*x603))+((new_r00*x602))+(((-1.0)*x604))); +evalcond[7]=((((-1.0)*x602*x604))+(((-1.0)*x606))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x609=((1.0)*new_r11); +CheckValue x610 = IKatan2WithCheck(IkReal((((gconst8*new_r01))+(((-1.0)*gconst7*x609)))),IkReal(((((-1.0)*gconst7*new_r01))+(((-1.0)*gconst8*x609)))),IKFAST_ATAN2_MAGTHRESH); +if(!x610.valid){ +continue; +} +CheckValue x611=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1); +if(!x611.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x610.value)+(((1.5707963267949)*(x611.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x612=IKcos(j3); +IkReal x613=IKsin(j3); +IkReal x614=((1.0)*gconst8); +IkReal x615=(gconst7*x612); +IkReal x616=(gconst7*x613); +IkReal x617=((1.0)*x613); +IkReal x618=(x613*x614); +evalcond[0]=(gconst7+((new_r11*x613))+((new_r01*x612))); +evalcond[1]=(((gconst8*x612))+x616+new_r11); +evalcond[2]=((((-1.0)*x618))+x615+new_r01); +evalcond[3]=(gconst7+((new_r10*x612))+(((-1.0)*new_r00*x617))); +evalcond[4]=(gconst8+((new_r11*x612))+(((-1.0)*new_r01*x617))); +evalcond[5]=((((-1.0)*x618))+x615+new_r10); +evalcond[6]=(((new_r10*x613))+(((-1.0)*x614))+((new_r00*x612))); +evalcond[7]=((((-1.0)*x612*x614))+(((-1.0)*x616))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x619=((1.0)*gconst7); +CheckValue x620 = IKatan2WithCheck(IkReal(((((-1.0)*new_r10*x619))+((gconst7*new_r01)))),IkReal(((((-1.0)*new_r11*x619))+(((-1.0)*new_r00*x619)))),IKFAST_ATAN2_MAGTHRESH); +if(!x620.valid){ +continue; +} +CheckValue x621=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1); +if(!x621.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x620.value)+(((1.5707963267949)*(x621.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x622=IKcos(j3); +IkReal x623=IKsin(j3); +IkReal x624=((1.0)*gconst8); +IkReal x625=(gconst7*x622); +IkReal x626=(gconst7*x623); +IkReal x627=((1.0)*x623); +IkReal x628=(x623*x624); +evalcond[0]=(gconst7+((new_r01*x622))+((new_r11*x623))); +evalcond[1]=(x626+((gconst8*x622))+new_r11); +evalcond[2]=((((-1.0)*x628))+x625+new_r01); +evalcond[3]=((((-1.0)*new_r00*x627))+gconst7+((new_r10*x622))); +evalcond[4]=((((-1.0)*new_r01*x627))+gconst8+((new_r11*x622))); +evalcond[5]=((((-1.0)*x628))+x625+new_r10); +evalcond[6]=((((-1.0)*x624))+((new_r00*x622))+((new_r10*x623))); +evalcond[7]=((((-1.0)*x622*x624))+(((-1.0)*x626))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +IkReal x630 = ((new_r01*new_r01)+(new_r11*new_r11)); +if(IKabs(x630)==0){ +continue; +} +IkReal x629=pow(x630,-0.5); +CheckValue x631 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x631.valid){ +continue; +} +IkReal gconst9=((3.14159265358979)+(((-1.0)*(x631.value)))); +IkReal gconst10=((1.0)*new_r01*x629); +IkReal gconst11=((-1.0)*new_r11*x629); +CheckValue x632 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x632.valid){ +continue; +} +evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x632.value)+j5)))), 6.28318530717959))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[3]; +CheckValue x635 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x635.valid){ +continue; +} +IkReal x633=((1.0)*(x635.value)); +IkReal x634=x629; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x633))); +IkReal gconst9=((3.14159265358979)+(((-1.0)*x633))); +IkReal gconst10=((1.0)*new_r01*x634); +IkReal gconst11=((-1.0)*new_r11*x634); +IkReal x636=new_r01*new_r01; +IkReal x637=(((new_r10*new_r11))+((new_r00*new_r01))); +IkReal x638=x629; +IkReal x639=((1.0)*new_r01*x638); +j3eval[0]=x637; +j3eval[1]=((IKabs(((((-1.0)*new_r00*x639))+(((-1.0)*new_r11*x639)))))+(IKabs((((x636*x638))+(((-1.0)*new_r10*x639)))))); +j3eval[2]=IKsign(x637); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[2]; +CheckValue x642 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x642.valid){ +continue; +} +IkReal x640=((1.0)*(x642.value)); +IkReal x641=x629; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x640))); +IkReal gconst9=((3.14159265358979)+(((-1.0)*x640))); +IkReal gconst10=((1.0)*new_r01*x641); +IkReal gconst11=((-1.0)*new_r11*x641); +IkReal x643=((new_r01*new_r01)+(new_r11*new_r11)); +j3eval[0]=x643; +j3eval[1]=IKsign(x643); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x646 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x646.valid){ +continue; +} +IkReal x644=((1.0)*(x646.value)); +IkReal x645=x629; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x644))); +IkReal gconst9=((3.14159265358979)+(((-1.0)*x644))); +IkReal gconst10=((1.0)*new_r01*x645); +IkReal gconst11=((-1.0)*new_r11*x645); +j3eval[0]=((new_r01*new_r01)+(new_r11*new_r11)); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal evalcond[5]; +bool bgotonextstatement = true; +do +{ +evalcond[0]=((gconst10*gconst10)+(gconst11*gconst11)); +evalcond[1]=new_r01; +evalcond[2]=new_r00; +evalcond[3]=new_r11; +evalcond[4]=new_r10; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[4], cj3array[4], sj3array[4]; +bool j3valid[4]={false}; +_nj3 = 4; +j3array[0]=0; +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=1.5707963267949; +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +j3array[2]=3.14159265358979; +sj3array[2]=IKsin(j3array[2]); +cj3array[2]=IKcos(j3array[2]); +j3array[3]=-1.5707963267949; +sj3array[3]=IKsin(j3array[3]); +cj3array[3]=IKcos(j3array[3]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +if( j3array[2] > IKPI ) +{ + j3array[2]-=IK2PI; +} +else if( j3array[2] < -IKPI ) +{ j3array[2]+=IK2PI; +} +j3valid[2] = true; +if( j3array[3] > IKPI ) +{ + j3array[3]-=IK2PI; +} +else if( j3array[3] < -IKPI ) +{ j3array[3]+=IK2PI; +} +j3valid[3] = true; +for(int ij3 = 0; ij3 < 4; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 4; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +CheckValue x648 = IKatan2WithCheck(IkReal(new_r01),IkReal(0),IKFAST_ATAN2_MAGTHRESH); +if(!x648.valid){ +continue; +} +IkReal x647=((1.0)*(x648.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x647))); +new_r11=0; +new_r00=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x647))); +IkReal x649 = new_r01*new_r01; +if(IKabs(x649)==0){ +continue; +} +IkReal gconst10=((1.0)*new_r01*(pow(x649,-0.5))); +IkReal gconst11=0; +j3eval[0]=new_r01; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x650=IKPowWithIntegerCheck(gconst10,-1); +if(!x650.valid){ +continue; +} +cj3array[0]=((-1.0)*new_r01*(x650.value)); +if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j3valid[0] = j3valid[1] = true; + j3array[0] = IKacos(cj3array[0]); + sj3array[0] = IKsin(j3array[0]); + cj3array[1] = cj3array[0]; + j3array[1] = -j3array[0]; + sj3array[1] = -sj3array[0]; +} +else if( isnan(cj3array[0]) ) +{ + // probably any value will work + j3valid[0] = true; + cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; +} +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x651=IKsin(j3); +IkReal x652=IKcos(j3); +IkReal x653=((-1.0)*x651); +evalcond[0]=(new_r10*x651); +evalcond[1]=(gconst10*x653); +evalcond[2]=(new_r01*x653); +evalcond[3]=(gconst10+((new_r01*x652))); +evalcond[4]=(gconst10+((new_r10*x652))); +evalcond[5]=(((gconst10*x652))+new_r10); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x654=IKPowWithIntegerCheck(new_r01,-1); +if(!x654.valid){ +continue; +} +cj3array[0]=((-1.0)*gconst10*(x654.value)); +if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j3valid[0] = j3valid[1] = true; + j3array[0] = IKacos(cj3array[0]); + sj3array[0] = IKsin(j3array[0]); + cj3array[1] = cj3array[0]; + j3array[1] = -j3array[0]; + sj3array[1] = -sj3array[0]; +} +else if( isnan(cj3array[0]) ) +{ + // probably any value will work + j3valid[0] = true; + cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; +} +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x655=IKsin(j3); +IkReal x656=IKcos(j3); +IkReal x657=(gconst10*x656); +IkReal x658=((-1.0)*x655); +evalcond[0]=(new_r10*x655); +evalcond[1]=(gconst10*x658); +evalcond[2]=(new_r01*x658); +evalcond[3]=(x657+new_r01); +evalcond[4]=(gconst10+((new_r10*x656))); +evalcond[5]=(x657+new_r10); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00))); +evalcond[1]=gconst10; +if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[4]; +CheckValue x660 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x660.valid){ +continue; +} +IkReal x659=((1.0)*(x660.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x659))); +new_r00=0; +new_r10=0; +new_r21=0; +new_r22=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x659))); +IkReal gconst10=((1.0)*new_r01); +IkReal gconst11=((-1.0)*new_r11); +j3eval[0]=1.0; +j3eval[1]=1.0; +j3eval[2]=new_r01; +j3eval[3]=1.0; +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +CheckValue x662 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x662.valid){ +continue; +} +IkReal x661=((1.0)*(x662.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x661))); +new_r00=0; +new_r10=0; +new_r21=0; +new_r22=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x661))); +IkReal gconst10=((1.0)*new_r01); +IkReal gconst11=((-1.0)*new_r11); +j3eval[0]=-1.0; +j3eval[1]=-1.0; +j3eval[2]=((IKabs(((-1.0)+(new_r01*new_r01))))+(IKabs(((1.0)*new_r01*new_r11)))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[3]; +CheckValue x664 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x664.valid){ +continue; +} +IkReal x663=((1.0)*(x664.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x663))); +new_r00=0; +new_r10=0; +new_r21=0; +new_r22=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x663))); +IkReal gconst10=((1.0)*new_r01); +IkReal gconst11=((-1.0)*new_r11); +j3eval[0]=1.0; +j3eval[1]=1.0; +j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((1.0)+(((-2.0)*(new_r01*new_r01))))))); +if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x665=((1.0)*new_r11); +CheckValue x666 = IKatan2WithCheck(IkReal(((((-1.0)*gconst10*x665))+((gconst11*new_r01)))),IkReal(((((-1.0)*gconst11*x665))+(((-1.0)*gconst10*new_r01)))),IKFAST_ATAN2_MAGTHRESH); +if(!x666.valid){ +continue; +} +CheckValue x667=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1); +if(!x667.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x666.value)+(((1.5707963267949)*(x667.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x668=IKcos(j3); +IkReal x669=IKsin(j3); +IkReal x670=(gconst10*x668); +IkReal x671=(gconst10*x669); +IkReal x672=(gconst11*x668); +IkReal x673=((1.0)*x669); +IkReal x674=(gconst11*x673); +evalcond[0]=(gconst10+((new_r11*x669))+((new_r01*x668))); +evalcond[1]=(x672+x671+new_r11); +evalcond[2]=(x670+(((-1.0)*x674))); +evalcond[3]=(x670+new_r01+(((-1.0)*x674))); +evalcond[4]=((((-1.0)*x671))+(((-1.0)*x672))); +evalcond[5]=((((-1.0)*new_r01*x673))+gconst11+((new_r11*x668))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x675=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1); +if(!x675.valid){ +continue; +} +CheckValue x676 = IKatan2WithCheck(IkReal((gconst10*new_r11)),IkReal((gconst11*new_r11)),IKFAST_ATAN2_MAGTHRESH); +if(!x676.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x675.value)))+(x676.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x677=IKcos(j3); +IkReal x678=IKsin(j3); +IkReal x679=(gconst10*x677); +IkReal x680=(gconst10*x678); +IkReal x681=(gconst11*x677); +IkReal x682=((1.0)*x678); +IkReal x683=(gconst11*x682); +evalcond[0]=(((new_r11*x678))+((new_r01*x677))+gconst10); +evalcond[1]=(x681+x680+new_r11); +evalcond[2]=((((-1.0)*x683))+x679); +evalcond[3]=((((-1.0)*x683))+x679+new_r01); +evalcond[4]=((((-1.0)*x681))+(((-1.0)*x680))); +evalcond[5]=(((new_r11*x677))+(((-1.0)*new_r01*x682))+gconst11); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x684=IKPowWithIntegerCheck(IKsign((((gconst10*new_r01))+(((-1.0)*gconst11*new_r11)))),-1); +if(!x684.valid){ +continue; +} +CheckValue x685 = IKatan2WithCheck(IkReal((gconst10*gconst11)),IkReal(((-1.0)*(gconst10*gconst10))),IKFAST_ATAN2_MAGTHRESH); +if(!x685.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x684.value)))+(x685.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x686=IKcos(j3); +IkReal x687=IKsin(j3); +IkReal x688=(gconst10*x686); +IkReal x689=(gconst10*x687); +IkReal x690=(gconst11*x686); +IkReal x691=((1.0)*x687); +IkReal x692=(gconst11*x691); +evalcond[0]=(gconst10+((new_r01*x686))+((new_r11*x687))); +evalcond[1]=(x689+x690+new_r11); +evalcond[2]=(x688+(((-1.0)*x692))); +evalcond[3]=(x688+(((-1.0)*x692))+new_r01); +evalcond[4]=((((-1.0)*x689))+(((-1.0)*x690))); +evalcond[5]=((((-1.0)*new_r01*x691))+gconst11+((new_r11*x686))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x693=IKPowWithIntegerCheck(gconst11,-1); +if(!x693.valid){ +continue; +} +cj3array[0]=(new_r00*(x693.value)); +if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH ) +{ + j3valid[0] = j3valid[1] = true; + j3array[0] = IKacos(cj3array[0]); + sj3array[0] = IKsin(j3array[0]); + cj3array[1] = cj3array[0]; + j3array[1] = -j3array[0]; + sj3array[1] = -sj3array[0]; +} +else if( isnan(cj3array[0]) ) +{ + // probably any value will work + j3valid[0] = true; + cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0; +} +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[6]; +IkReal x694=IKsin(j3); +IkReal x695=IKcos(j3); +IkReal x696=((-1.0)*x694); +evalcond[0]=(new_r11*x694); +evalcond[1]=(gconst11*x696); +evalcond[2]=(new_r00*x696); +evalcond[3]=(((gconst11*x695))+new_r11); +evalcond[4]=(gconst11+((new_r11*x695))); +evalcond[5]=(((new_r00*x695))+(((-1.0)*gconst11))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r00))+(IKabs(new_r01))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +CheckValue x698 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x698.valid){ +continue; +} +IkReal x697=((1.0)*(x698.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x697))); +new_r00=0; +new_r01=0; +new_r12=0; +new_r22=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x697))); +IkReal gconst10=0; +IkReal x699 = ((1.0)+(((-1.0)*(new_r10*new_r10)))); +if(IKabs(x699)==0){ +continue; +} +IkReal gconst11=((-1.0)*new_r11*(pow(x699,-0.5))); +j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x701 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x701.valid){ +continue; +} +IkReal x700=((1.0)*(x701.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x700))); +new_r00=0; +new_r01=0; +new_r12=0; +new_r22=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x700))); +IkReal gconst10=0; +IkReal x702 = ((1.0)+(((-1.0)*(new_r10*new_r10)))); +if(IKabs(x702)==0){ +continue; +} +IkReal gconst11=((-1.0)*new_r11*(pow(x702,-0.5))); +j3eval[0]=new_r11; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x704 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x704.valid){ +continue; +} +IkReal x703=((1.0)*(x704.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x703))); +new_r00=0; +new_r01=0; +new_r12=0; +new_r22=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x703))); +IkReal gconst10=0; +IkReal x705 = ((1.0)+(((-1.0)*(new_r10*new_r10)))); +if(IKabs(x705)==0){ +continue; +} +IkReal gconst11=((-1.0)*new_r11*(pow(x705,-0.5))); +j3eval[0]=new_r10; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x706=IKPowWithIntegerCheck(new_r10,-1); +if(!x706.valid){ +continue; +} +CheckValue x707=IKPowWithIntegerCheck(gconst11,-1); +if(!x707.valid){ +continue; +} +if( IKabs((gconst11*(x706.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11*(x707.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((gconst11*(x706.value)))+IKsqr(((-1.0)*new_r11*(x707.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((gconst11*(x706.value)), ((-1.0)*new_r11*(x707.value))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x708=IKsin(j3); +IkReal x709=IKcos(j3); +IkReal x710=(gconst11*x709); +IkReal x711=(gconst11*x708); +evalcond[0]=(new_r11*x708); +evalcond[1]=(new_r10*x709); +evalcond[2]=((-1.0)*x711); +evalcond[3]=((-1.0)*x710); +evalcond[4]=(x710+new_r11); +evalcond[5]=(gconst11+((new_r11*x709))); +evalcond[6]=((((-1.0)*x711))+new_r10); +evalcond[7]=((((-1.0)*gconst11))+((new_r10*x708))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x712=IKPowWithIntegerCheck(gconst11,-1); +if(!x712.valid){ +continue; +} +CheckValue x713=IKPowWithIntegerCheck(new_r11,-1); +if(!x713.valid){ +continue; +} +if( IKabs((new_r10*(x712.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst11*(x713.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x712.value)))+IKsqr(((-1.0)*gconst11*(x713.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((new_r10*(x712.value)), ((-1.0)*gconst11*(x713.value))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x714=IKsin(j3); +IkReal x715=IKcos(j3); +IkReal x716=(gconst11*x715); +IkReal x717=(gconst11*x714); +evalcond[0]=(new_r11*x714); +evalcond[1]=(new_r10*x715); +evalcond[2]=((-1.0)*x717); +evalcond[3]=((-1.0)*x716); +evalcond[4]=(x716+new_r11); +evalcond[5]=(gconst11+((new_r11*x715))); +evalcond[6]=((((-1.0)*x717))+new_r10); +evalcond[7]=((((-1.0)*gconst11))+((new_r10*x714))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x718=IKPowWithIntegerCheck(IKsign(gconst11),-1); +if(!x718.valid){ +continue; +} +CheckValue x719 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH); +if(!x719.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x718.value)))+(x719.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x720=IKsin(j3); +IkReal x721=IKcos(j3); +IkReal x722=(gconst11*x721); +IkReal x723=(gconst11*x720); +evalcond[0]=(new_r11*x720); +evalcond[1]=(new_r10*x721); +evalcond[2]=((-1.0)*x723); +evalcond[3]=((-1.0)*x722); +evalcond[4]=(x722+new_r11); +evalcond[5]=(gconst11+((new_r11*x721))); +evalcond[6]=((((-1.0)*x723))+new_r10); +evalcond[7]=((((-1.0)*gconst11))+((new_r10*x720))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=IKabs(new_r01); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +CheckValue x725 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x725.valid){ +continue; +} +IkReal x724=((1.0)*(x725.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x724))); +new_r01=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x724))); +IkReal gconst10=0; +IkReal x726 = new_r11*new_r11; +if(IKabs(x726)==0){ +continue; +} +IkReal gconst11=((-1.0)*new_r11*(pow(x726,-0.5))); +j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x728 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x728.valid){ +continue; +} +IkReal x727=((1.0)*(x728.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x727))); +new_r01=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x727))); +IkReal gconst10=0; +IkReal x729 = new_r11*new_r11; +if(IKabs(x729)==0){ +continue; +} +IkReal gconst11=((-1.0)*new_r11*(pow(x729,-0.5))); +j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +CheckValue x731 = IKatan2WithCheck(IkReal(0),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x731.valid){ +continue; +} +IkReal x730=((1.0)*(x731.value)); +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +sj5=gconst10; +cj5=gconst11; +j5=((3.14159265)+(((-1.0)*x730))); +new_r01=0; +IkReal gconst9=((3.14159265358979)+(((-1.0)*x730))); +IkReal gconst10=0; +IkReal x732 = new_r11*new_r11; +if(IKabs(x732)==0){ +continue; +} +IkReal gconst11=((-1.0)*new_r11*(pow(x732,-0.5))); +j3eval[0]=new_r11; +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // 3 cases reached + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x733=IKPowWithIntegerCheck(gconst11,-1); +if(!x733.valid){ +continue; +} +CheckValue x734=IKPowWithIntegerCheck(new_r11,-1); +if(!x734.valid){ +continue; +} +if( IKabs((new_r10*(x733.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*gconst11*(x734.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r10*(x733.value)))+IKsqr(((-1.0)*gconst11*(x734.value)))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((new_r10*(x733.value)), ((-1.0)*gconst11*(x734.value))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x735=IKsin(j3); +IkReal x736=IKcos(j3); +IkReal x737=(gconst11*x736); +IkReal x738=((1.0)*x735); +evalcond[0]=(new_r11*x735); +evalcond[1]=((-1.0)*gconst11*x735); +evalcond[2]=(x737+new_r11); +evalcond[3]=(gconst11+((new_r11*x736))); +evalcond[4]=((((-1.0)*x737))+new_r00); +evalcond[5]=((((-1.0)*gconst11*x738))+new_r10); +evalcond[6]=((((-1.0)*new_r00*x738))+((new_r10*x736))); +evalcond[7]=(((new_r00*x736))+(((-1.0)*gconst11))+((new_r10*x735))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x739=IKPowWithIntegerCheck(IKsign(gconst11),-1); +if(!x739.valid){ +continue; +} +CheckValue x740 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r11)),IKFAST_ATAN2_MAGTHRESH); +if(!x740.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x739.value)))+(x740.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x741=IKsin(j3); +IkReal x742=IKcos(j3); +IkReal x743=(gconst11*x742); +IkReal x744=((1.0)*x741); +evalcond[0]=(new_r11*x741); +evalcond[1]=((-1.0)*gconst11*x741); +evalcond[2]=(x743+new_r11); +evalcond[3]=(gconst11+((new_r11*x742))); +evalcond[4]=((((-1.0)*x743))+new_r00); +evalcond[5]=((((-1.0)*gconst11*x744))+new_r10); +evalcond[6]=((((-1.0)*new_r00*x744))+((new_r10*x742))); +evalcond[7]=(((new_r10*x741))+((new_r00*x742))+(((-1.0)*gconst11))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x745=IKPowWithIntegerCheck(IKsign(gconst11),-1); +if(!x745.valid){ +continue; +} +CheckValue x746 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH); +if(!x746.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x745.value)))+(x746.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x747=IKsin(j3); +IkReal x748=IKcos(j3); +IkReal x749=(gconst11*x748); +IkReal x750=((1.0)*x747); +evalcond[0]=(new_r11*x747); +evalcond[1]=((-1.0)*gconst11*x747); +evalcond[2]=(x749+new_r11); +evalcond[3]=(gconst11+((new_r11*x748))); +evalcond[4]=((((-1.0)*x749))+new_r00); +evalcond[5]=((((-1.0)*gconst11*x750))+new_r10); +evalcond[6]=((((-1.0)*new_r00*x750))+((new_r10*x748))); +evalcond[7]=(((new_r10*x747))+((new_r00*x748))+(((-1.0)*gconst11))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j3] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x751=((1.0)*new_r11); +CheckValue x752 = IKatan2WithCheck(IkReal((((gconst11*new_r01))+(((-1.0)*gconst10*x751)))),IkReal(((((-1.0)*gconst11*x751))+(((-1.0)*gconst10*new_r01)))),IKFAST_ATAN2_MAGTHRESH); +if(!x752.valid){ +continue; +} +CheckValue x753=IKPowWithIntegerCheck(IKsign(((gconst10*gconst10)+(gconst11*gconst11))),-1); +if(!x753.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x752.value)+(((1.5707963267949)*(x753.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x754=IKcos(j3); +IkReal x755=IKsin(j3); +IkReal x756=(gconst10*x754); +IkReal x757=(gconst11*x754); +IkReal x758=(gconst10*x755); +IkReal x759=((1.0)*x755); +IkReal x760=(gconst11*x759); +evalcond[0]=(gconst10+((new_r11*x755))+((new_r01*x754))); +evalcond[1]=(x757+x758+new_r11); +evalcond[2]=((((-1.0)*x760))+x756+new_r01); +evalcond[3]=((((-1.0)*new_r00*x759))+gconst10+((new_r10*x754))); +evalcond[4]=((((-1.0)*new_r01*x759))+gconst11+((new_r11*x754))); +evalcond[5]=((((-1.0)*x760))+x756+new_r10); +evalcond[6]=(((new_r00*x754))+((new_r10*x755))+(((-1.0)*gconst11))); +evalcond[7]=((((-1.0)*x758))+(((-1.0)*x757))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x761=((1.0)*new_r11); +CheckValue x762=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1); +if(!x762.valid){ +continue; +} +CheckValue x763 = IKatan2WithCheck(IkReal((((gconst11*new_r01))+(((-1.0)*gconst10*x761)))),IkReal(((((-1.0)*gconst11*x761))+(((-1.0)*gconst10*new_r01)))),IKFAST_ATAN2_MAGTHRESH); +if(!x763.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x762.value)))+(x763.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x764=IKcos(j3); +IkReal x765=IKsin(j3); +IkReal x766=(gconst10*x764); +IkReal x767=(gconst11*x764); +IkReal x768=(gconst10*x765); +IkReal x769=((1.0)*x765); +IkReal x770=(gconst11*x769); +evalcond[0]=(((new_r11*x765))+gconst10+((new_r01*x764))); +evalcond[1]=(x768+x767+new_r11); +evalcond[2]=(x766+(((-1.0)*x770))+new_r01); +evalcond[3]=(((new_r10*x764))+(((-1.0)*new_r00*x769))+gconst10); +evalcond[4]=(((new_r11*x764))+gconst11+(((-1.0)*new_r01*x769))); +evalcond[5]=(x766+(((-1.0)*x770))+new_r10); +evalcond[6]=(((new_r10*x765))+((new_r00*x764))+(((-1.0)*gconst11))); +evalcond[7]=((((-1.0)*x767))+(((-1.0)*x768))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x771=((1.0)*gconst10); +CheckValue x772 = IKatan2WithCheck(IkReal(((((-1.0)*new_r10*x771))+((gconst10*new_r01)))),IkReal(((((-1.0)*new_r00*x771))+(((-1.0)*new_r11*x771)))),IKFAST_ATAN2_MAGTHRESH); +if(!x772.valid){ +continue; +} +CheckValue x773=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1); +if(!x773.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x772.value)+(((1.5707963267949)*(x773.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x774=IKcos(j3); +IkReal x775=IKsin(j3); +IkReal x776=(gconst10*x774); +IkReal x777=(gconst11*x774); +IkReal x778=(gconst10*x775); +IkReal x779=((1.0)*x775); +IkReal x780=(gconst11*x779); +evalcond[0]=(gconst10+((new_r11*x775))+((new_r01*x774))); +evalcond[1]=(x777+x778+new_r11); +evalcond[2]=((((-1.0)*x780))+x776+new_r01); +evalcond[3]=((((-1.0)*new_r00*x779))+gconst10+((new_r10*x774))); +evalcond[4]=((((-1.0)*new_r01*x779))+gconst11+((new_r11*x774))); +evalcond[5]=((((-1.0)*x780))+x776+new_r10); +evalcond[6]=(((new_r00*x774))+((new_r10*x775))+(((-1.0)*gconst11))); +evalcond[7]=(new_r00+(((-1.0)*x778))+(((-1.0)*x777))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +sj4=0; +cj4=-1.0; +j4=3.14159265358979; +new_r11=0; +new_r01=0; +new_r22=0; +new_r20=0; +j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // no branches [j3] + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x782 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH); +if(!x782.valid){ +continue; +} +IkReal x781=x782.value; +j3array[0]=((-1.0)*x781); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((3.14159265358979)+(((-1.0)*x781))); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[1]; +evalcond[0]=((((-1.0)*new_r00*(IKsin(j3))))+((new_r10*(IKcos(j3))))); +if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j3] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} +} +} +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x783=((1.0)*sj5); +CheckValue x784 = IKatan2WithCheck(IkReal((((cj5*new_r01))+(((-1.0)*new_r11*x783)))),IkReal(((((-1.0)*new_r01*x783))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH); +if(!x784.valid){ +continue; +} +CheckValue x785=IKPowWithIntegerCheck(IKsign(((new_r01*new_r01)+(new_r11*new_r11))),-1); +if(!x785.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x784.value)+(((1.5707963267949)*(x785.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x786=IKsin(j3); +IkReal x787=IKcos(j3); +IkReal x788=((1.0)*cj5); +IkReal x789=(sj5*x787); +IkReal x790=(sj5*x786); +IkReal x791=((1.0)*x786); +IkReal x792=(x786*x788); +evalcond[0]=(sj5+((new_r11*x786))+((new_r01*x787))); +evalcond[1]=(((cj5*x787))+x790+new_r11); +evalcond[2]=((((-1.0)*x792))+x789+new_r01); +evalcond[3]=(sj5+((new_r10*x787))+(((-1.0)*new_r00*x791))); +evalcond[4]=((((-1.0)*new_r01*x791))+cj5+((new_r11*x787))); +evalcond[5]=((((-1.0)*x792))+x789+new_r10); +evalcond[6]=(((new_r10*x786))+(((-1.0)*x788))+((new_r00*x787))); +evalcond[7]=((((-1.0)*x787*x788))+(((-1.0)*x790))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +IkReal x793=((1.0)*sj5); +CheckValue x794 = IKatan2WithCheck(IkReal((((new_r01*sj5))+(((-1.0)*new_r10*x793)))),IkReal(((((-1.0)*new_r11*x793))+(((-1.0)*new_r00*x793)))),IKFAST_ATAN2_MAGTHRESH); +if(!x794.valid){ +continue; +} +CheckValue x795=IKPowWithIntegerCheck(IKsign((((new_r10*new_r11))+((new_r00*new_r01)))),-1); +if(!x795.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x794.value)+(((1.5707963267949)*(x795.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x796=IKsin(j3); +IkReal x797=IKcos(j3); +IkReal x798=((1.0)*cj5); +IkReal x799=(sj5*x797); +IkReal x800=(sj5*x796); +IkReal x801=((1.0)*x796); +IkReal x802=(x796*x798); +evalcond[0]=(sj5+((new_r11*x796))+((new_r01*x797))); +evalcond[1]=(((cj5*x797))+new_r11+x800); +evalcond[2]=(x799+new_r01+(((-1.0)*x802))); +evalcond[3]=(sj5+((new_r10*x797))+(((-1.0)*new_r00*x801))); +evalcond[4]=((((-1.0)*new_r01*x801))+cj5+((new_r11*x797))); +evalcond[5]=(x799+new_r10+(((-1.0)*x802))); +evalcond[6]=((((-1.0)*x798))+((new_r10*x796))+((new_r00*x797))); +evalcond[7]=((((-1.0)*x800))+(((-1.0)*x797*x798))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x803 = IKatan2WithCheck(IkReal(((-1.0)+(new_r01*new_r01)+(cj5*cj5))),IkReal(((((-1.0)*cj5*sj5))+(((-1.0)*new_r01*new_r11)))),IKFAST_ATAN2_MAGTHRESH); +if(!x803.valid){ +continue; +} +CheckValue x804=IKPowWithIntegerCheck(IKsign((((new_r11*sj5))+((cj5*new_r01)))),-1); +if(!x804.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(x803.value)+(((1.5707963267949)*(x804.value)))); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[8]; +IkReal x805=IKsin(j3); +IkReal x806=IKcos(j3); +IkReal x807=((1.0)*cj5); +IkReal x808=(sj5*x806); +IkReal x809=(sj5*x805); +IkReal x810=((1.0)*x805); +IkReal x811=(x805*x807); +evalcond[0]=(sj5+((new_r11*x805))+((new_r01*x806))); +evalcond[1]=(((cj5*x806))+new_r11+x809); +evalcond[2]=((((-1.0)*x811))+new_r01+x808); +evalcond[3]=(sj5+(((-1.0)*new_r00*x810))+((new_r10*x806))); +evalcond[4]=(cj5+(((-1.0)*new_r01*x810))+((new_r11*x806))); +evalcond[5]=((((-1.0)*x811))+new_r10+x808); +evalcond[6]=(((new_r00*x806))+((new_r10*x805))+(((-1.0)*x807))); +evalcond[7]=((((-1.0)*x809))+(((-1.0)*x806*x807))+new_r00); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02))); +if( IKabs(evalcond[0]) < 0.0000050000000000 ) +{ +bgotonextstatement=false; +{ +IkReal j3eval[1]; +new_r02=0; +new_r12=0; +new_r20=0; +new_r21=0; +j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +new_r02=0; +new_r12=0; +new_r20=0; +new_r21=0; +j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +{ +IkReal j3eval[1]; +new_r02=0; +new_r12=0; +new_r20=0; +new_r21=0; +j3eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22)))); +if( IKabs(j3eval[0]) < 0.0000010000000000 ) +{ +continue; // no branches [j3] + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x813 = IKatan2WithCheck(IkReal((new_r10*new_r22)),IkReal(((-1.0)*new_r00*new_r22)),IKFAST_ATAN2_MAGTHRESH); +if(!x813.valid){ +continue; +} +IkReal x812=x813.value; +j3array[0]=((-1.0)*x812); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((3.14159265358979)+(((-1.0)*x812))); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[5]; +IkReal x814=IKsin(j3); +IkReal x815=IKcos(j3); +IkReal x816=(new_r11*x815); +IkReal x817=((1.0)*x814); +evalcond[0]=(((new_r11*x814))+((new_r01*x815))); +evalcond[1]=(((new_r00*x815))+((new_r10*x814))); +evalcond[2]=(((new_r10*x815))+(((-1.0)*new_r00*x817))); +evalcond[3]=((((-1.0)*new_r01*x817))+x816); +evalcond[4]=(((new_r22*x816))+(((-1.0)*new_r01*new_r22*x817))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x819 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH); +if(!x819.valid){ +continue; +} +IkReal x818=x819.value; +j3array[0]=((-1.0)*x818); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((3.14159265358979)+(((-1.0)*x818))); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[5]; +IkReal x820=IKcos(j3); +IkReal x821=IKsin(j3); +IkReal x822=(new_r11*x820); +IkReal x823=(new_r10*x820); +IkReal x824=((1.0)*x821); +evalcond[0]=(((new_r01*x820))+((new_r11*x821))); +evalcond[1]=((((-1.0)*new_r00*x824))+x823); +evalcond[2]=((((-1.0)*new_r01*x824))+x822); +evalcond[3]=((((-1.0)*new_r00*new_r22*x824))+((new_r22*x823))); +evalcond[4]=(((new_r22*x822))+(((-1.0)*new_r01*new_r22*x824))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[2], cj3array[2], sj3array[2]; +bool j3valid[2]={false}; +_nj3 = 2; +CheckValue x826 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH); +if(!x826.valid){ +continue; +} +IkReal x825=x826.value; +j3array[0]=((-1.0)*x825); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +j3array[1]=((3.14159265358979)+(((-1.0)*x825))); +sj3array[1]=IKsin(j3array[1]); +cj3array[1]=IKcos(j3array[1]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +if( j3array[1] > IKPI ) +{ + j3array[1]-=IK2PI; +} +else if( j3array[1] < -IKPI ) +{ j3array[1]+=IK2PI; +} +j3valid[1] = true; +for(int ij3 = 0; ij3 < 2; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 2; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[5]; +IkReal x827=IKcos(j3); +IkReal x828=IKsin(j3); +IkReal x829=(new_r11*x827); +IkReal x830=(new_r10*x827); +IkReal x831=((1.0)*x828); +evalcond[0]=(((new_r10*x828))+((new_r00*x827))); +evalcond[1]=(x830+(((-1.0)*new_r00*x831))); +evalcond[2]=((((-1.0)*new_r01*x831))+x829); +evalcond[3]=((((-1.0)*new_r00*new_r22*x831))+((new_r22*x830))); +evalcond[4]=(((new_r22*x829))+(((-1.0)*new_r01*new_r22*x831))); +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(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} +} while(0); +if( bgotonextstatement ) +{ +bool bgotonextstatement = true; +do +{ +if( 1 ) +{ +bgotonextstatement=false; +continue; // branch miss [j3] + +} +} while(0); +if( bgotonextstatement ) +{ +} +} +} +} +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x833=IKPowWithIntegerCheck(sj4,-1); +if(!x833.valid){ +continue; +} +IkReal x832=x833.value; +CheckValue x834=IKPowWithIntegerCheck(new_r11,-1); +if(!x834.valid){ +continue; +} +if( IKabs((x832*(x834.value)*((((new_r01*new_r12))+(((-1.0)*sj4*sj5)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r12*x832)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x832*(x834.value)*((((new_r01*new_r12))+(((-1.0)*sj4*sj5))))))+IKsqr(((-1.0)*new_r12*x832))-1) <= IKFAST_SINCOS_THRESH ) + continue; +j3array[0]=IKatan2((x832*(x834.value)*((((new_r01*new_r12))+(((-1.0)*sj4*sj5))))), ((-1.0)*new_r12*x832)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[18]; +IkReal x835=IKsin(j3); +IkReal x836=IKcos(j3); +IkReal x837=((1.0)*cj5); +IkReal x838=((1.0)*cj4); +IkReal x839=(new_r11*x836); +IkReal x840=(sj5*x836); +IkReal x841=(cj4*x836); +IkReal x842=(sj4*x836); +IkReal x843=(new_r00*x835); +IkReal x844=(cj4*x835); +IkReal x845=(new_r01*x835); +IkReal x846=(new_r02*x835); +IkReal x847=((1.0)*sj4*x835); +evalcond[0]=(new_r12+x842); +evalcond[1]=((((-1.0)*x847))+new_r02); +evalcond[2]=(((new_r12*x835))+((new_r02*x836))); +evalcond[3]=(((new_r11*x835))+sj5+((new_r01*x836))); +evalcond[4]=(sj4+((new_r12*x836))+(((-1.0)*x846))); +evalcond[5]=(((cj5*x844))+new_r01+x840); +evalcond[6]=(((new_r00*x836))+(((-1.0)*x837))+((new_r10*x835))); +evalcond[7]=((((-1.0)*x836*x837))+((sj5*x844))+new_r00); +evalcond[8]=((((-1.0)*x837*x841))+((sj5*x835))+new_r11); +evalcond[9]=((((-1.0)*x835*x837))+(((-1.0)*x838*x840))+new_r10); +evalcond[10]=((((-1.0)*x843))+(((-1.0)*sj5*x838))+((new_r10*x836))); +evalcond[11]=((((-1.0)*cj4*x837))+(((-1.0)*x845))+x839); +evalcond[12]=(((new_r12*x841))+((new_r22*sj4))+(((-1.0)*x838*x846))); +evalcond[13]=(((new_r10*x842))+(((-1.0)*sj4*x843))+(((-1.0)*new_r20*x838))); +evalcond[14]=(((sj4*x839))+(((-1.0)*sj4*x845))+(((-1.0)*new_r21*x838))); +evalcond[15]=((1.0)+(((-1.0)*new_r22*x838))+((new_r12*x842))+(((-1.0)*sj4*x846))); +evalcond[16]=(((new_r10*x841))+(((-1.0)*sj5))+((new_r20*sj4))+(((-1.0)*x838*x843))); +evalcond[17]=(((cj4*x839))+(((-1.0)*x837))+(((-1.0)*x838*x845))+((new_r21*sj4))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} + +} else +{ +{ +IkReal j3array[1], cj3array[1], sj3array[1]; +bool j3valid[1]={false}; +_nj3 = 1; +CheckValue x848=IKPowWithIntegerCheck(IKsign(sj4),-1); +if(!x848.valid){ +continue; +} +CheckValue x849 = IKatan2WithCheck(IkReal(new_r02),IkReal(((-1.0)*new_r12)),IKFAST_ATAN2_MAGTHRESH); +if(!x849.valid){ +continue; +} +j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x848.value)))+(x849.value)); +sj3array[0]=IKsin(j3array[0]); +cj3array[0]=IKcos(j3array[0]); +if( j3array[0] > IKPI ) +{ + j3array[0]-=IK2PI; +} +else if( j3array[0] < -IKPI ) +{ j3array[0]+=IK2PI; +} +j3valid[0] = true; +for(int ij3 = 0; ij3 < 1; ++ij3) +{ +if( !j3valid[ij3] ) +{ + continue; +} +_ij3[0] = ij3; _ij3[1] = -1; +for(int iij3 = ij3+1; iij3 < 1; ++iij3) +{ +if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH ) +{ + j3valid[iij3]=false; _ij3[1] = iij3; break; +} +} +j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3]; +{ +IkReal evalcond[18]; +IkReal x850=IKsin(j3); +IkReal x851=IKcos(j3); +IkReal x852=((1.0)*cj5); +IkReal x853=((1.0)*cj4); +IkReal x854=(new_r11*x851); +IkReal x855=(sj5*x851); +IkReal x856=(cj4*x851); +IkReal x857=(sj4*x851); +IkReal x858=(new_r00*x850); +IkReal x859=(cj4*x850); +IkReal x860=(new_r01*x850); +IkReal x861=(new_r02*x850); +IkReal x862=((1.0)*sj4*x850); +evalcond[0]=(new_r12+x857); +evalcond[1]=((((-1.0)*x862))+new_r02); +evalcond[2]=(((new_r12*x850))+((new_r02*x851))); +evalcond[3]=(sj5+((new_r11*x850))+((new_r01*x851))); +evalcond[4]=(sj4+(((-1.0)*x861))+((new_r12*x851))); +evalcond[5]=(new_r01+x855+((cj5*x859))); +evalcond[6]=((((-1.0)*x852))+((new_r10*x850))+((new_r00*x851))); +evalcond[7]=(((sj5*x859))+new_r00+(((-1.0)*x851*x852))); +evalcond[8]=(((sj5*x850))+(((-1.0)*x852*x856))+new_r11); +evalcond[9]=((((-1.0)*x853*x855))+(((-1.0)*x850*x852))+new_r10); +evalcond[10]=((((-1.0)*x858))+(((-1.0)*sj5*x853))+((new_r10*x851))); +evalcond[11]=((((-1.0)*x860))+x854+(((-1.0)*cj4*x852))); +evalcond[12]=((((-1.0)*x853*x861))+((new_r22*sj4))+((new_r12*x856))); +evalcond[13]=((((-1.0)*sj4*x858))+(((-1.0)*new_r20*x853))+((new_r10*x857))); +evalcond[14]=((((-1.0)*new_r21*x853))+((sj4*x854))+(((-1.0)*sj4*x860))); +evalcond[15]=((1.0)+((new_r12*x857))+(((-1.0)*new_r22*x853))+(((-1.0)*sj4*x861))); +evalcond[16]=((((-1.0)*sj5))+(((-1.0)*x853*x858))+((new_r20*sj4))+((new_r10*x856))); +evalcond[17]=((((-1.0)*x852))+(((-1.0)*x853*x860))+((new_r21*sj4))+((cj4*x854))); +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 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH ) +{ +continue; +} +} + +{ +std::vector > vinfos(6); +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; +vinfos[3].jointtype = 1; +vinfos[3].foffset = j3; +vinfos[3].indices[0] = _ij3[0]; +vinfos[3].indices[1] = _ij3[1]; +vinfos[3].maxsolutions = _nj3; +vinfos[4].jointtype = 1; +vinfos[4].foffset = j4; +vinfos[4].indices[0] = _ij4[0]; +vinfos[4].indices[1] = _ij4[1]; +vinfos[4].maxsolutions = _nj4; +vinfos[5].jointtype = 1; +vinfos[5].foffset = j5; +vinfos[5].indices[0] = _ij5[0]; +vinfos[5].indices[1] = _ij5[1]; +vinfos[5].maxsolutions = _nj5; +std::vector vfree(0); +solutions.AddSolution(vinfos,vfree); +} +} +} + +} + +} +} +} + +} + +} +} +} +} +}static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots) +{ + using std::complex; + if( rawcoeffs[0] == 0 ) { + // solve with one reduced degree + polyroots2(&rawcoeffs[1], &rawroots[0], numroots); + return; + } + IKFAST_ASSERT(rawcoeffs[0] != 0); + const IkReal tol = 128.0*std::numeric_limits::epsilon(); + const IkReal tolsqrt = sqrt(std::numeric_limits::epsilon()); + complex coeffs[3]; + const int maxsteps = 110; + for(int i = 0; i < 3; ++i) { + coeffs[i] = complex(rawcoeffs[i+1]/rawcoeffs[0]); + } + complex roots[3]; + IkReal err[3]; + roots[0] = complex(1,0); + roots[1] = complex(0.4,0.9); // any complex number not a root of unity works + err[0] = 1.0; + err[1] = 1.0; + for(int i = 2; i < 3; ++i) { + roots[i] = roots[i-1]*roots[1]; + err[i] = 1.0; + } + for(int step = 0; step < maxsteps; ++step) { + bool changed = false; + for(int i = 0; i < 3; ++i) { + if ( err[i] >= tol ) { + changed = true; + // evaluate + complex x = roots[i] + coeffs[0]; + for(int j = 1; j < 3; ++j) { + x = roots[i] * x + coeffs[j]; + } + for(int j = 0; j < 3; ++j) { + if( i != j ) { + if( roots[i] != roots[j] ) { + x /= (roots[i] - roots[j]); + } + } + } + roots[i] -= x; + err[i] = abs(x); + } + } + if( !changed ) { + break; + } + } + + numroots = 0; + bool visited[3] = {false}; + for(int i = 0; i < 3; ++i) { + if( !visited[i] ) { + // might be a multiple root, in which case it will have more error than the other roots + // find any neighboring roots, and take the average + complex newroot=roots[i]; + int n = 1; + for(int j = i+1; j < 3; ++j) { + // care about error in real much more than imaginary + if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) { + newroot += roots[j]; + n += 1; + visited[j] = true; + } + } + if( n > 1 ) { + newroot /= n; + } + // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt + if( IKabs(imag(newroot)) < tolsqrt ) { + rawroots[numroots++] = real(newroot); + } + } + } +} +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; + } +} +static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots) +{ + using std::complex; + if( rawcoeffs[0] == 0 ) { + // solve with one reduced degree + polyroots3(&rawcoeffs[1], &rawroots[0], numroots); + return; + } + IKFAST_ASSERT(rawcoeffs[0] != 0); + const IkReal tol = 128.0*std::numeric_limits::epsilon(); + const IkReal tolsqrt = sqrt(std::numeric_limits::epsilon()); + complex coeffs[4]; + const int maxsteps = 110; + for(int i = 0; i < 4; ++i) { + coeffs[i] = complex(rawcoeffs[i+1]/rawcoeffs[0]); + } + complex roots[4]; + IkReal err[4]; + roots[0] = complex(1,0); + roots[1] = complex(0.4,0.9); // any complex number not a root of unity works + err[0] = 1.0; + err[1] = 1.0; + for(int i = 2; i < 4; ++i) { + roots[i] = roots[i-1]*roots[1]; + err[i] = 1.0; + } + for(int step = 0; step < maxsteps; ++step) { + bool changed = false; + for(int i = 0; i < 4; ++i) { + if ( err[i] >= tol ) { + changed = true; + // evaluate + complex x = roots[i] + coeffs[0]; + for(int j = 1; j < 4; ++j) { + x = roots[i] * x + coeffs[j]; + } + for(int j = 0; j < 4; ++j) { + if( i != j ) { + if( roots[i] != roots[j] ) { + x /= (roots[i] - roots[j]); + } + } + } + roots[i] -= x; + err[i] = abs(x); + } + } + if( !changed ) { + break; + } + } + + numroots = 0; + bool visited[4] = {false}; + for(int i = 0; i < 4; ++i) { + if( !visited[i] ) { + // might be a multiple root, in which case it will have more error than the other roots + // find any neighboring roots, and take the average + complex newroot=roots[i]; + int n = 1; + for(int j = i+1; j < 4; ++j) { + // care about error in real much more than imaginary + if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) { + newroot += roots[j]; + n += 1; + visited[j] = true; + } + } + if( n > 1 ) { + newroot /= n; + } + // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt + if( IKabs(imag(newroot)) < tolsqrt ) { + rawroots[numroots++] = real(newroot); + } + } + } +} +}; + + +/// 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 ""; } + +IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; } + +#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