From f9e64d963e8769211ef0e69c605f05ba247bebb5 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Fri, 15 Jan 2016 15:43:52 -0600 Subject: [PATCH] Ran clang-format on every cpp, h, and hpp file in the godel repo. --- .../Keyence_tcp_client.h | 29 +- godel_keyence_ljv_driver/src/driver.cpp | 124 +- godel_keyence_ljv_driver/src/ljv7_rawdata.cpp | 32 +- godel_keyence_ljv_driver/src/ljv7_rawdata.h | 7 +- .../godel_param_helpers/godel_param_helpers.h | 19 +- .../path_execution_service.h | 3 +- .../src/path_execution_service.cpp | 22 +- .../rqt_plugins/robot_blending_plugin.h | 52 +- .../rviz_panels/robot_blending_panel.h | 46 +- .../widgets/blend_tool_param_window.h | 2 - .../widgets/parameter_window_base.h | 2 - .../widgets/robot_blending_widget.h | 213 +- .../widgets/robot_scan_configuration.h | 20 +- .../widgets/scan_tool_configuration_window.h | 2 - .../widgets/surface_detection_configuration.h | 3 - .../src/rqt_plugins/robot_blending_plugin.cpp | 56 +- .../src/rviz_panels/robot_blending_panel.cpp | 75 +- .../src/widgets/blend_tool_param_window.cpp | 6 +- .../src/widgets/parameter_window_base.cpp | 5 +- .../src/widgets/robot_blending_widget.cpp | 926 +- .../src/widgets/robot_scan_configuration.cpp | 48 +- .../scan_tool_configuration_window.cpp | 12 +- .../surface_detection_configuration.cpp | 19 +- .../godel_polygon_offset/polygon_offset.h | 40 +- godel_polygon_offset/src/polygon_offset.cpp | 63 +- .../src/polygon_offset_node.cpp | 7 +- .../abb_blend_process_service.h | 1 - .../blend_process_service.h | 1 - .../keyence_process_service.h | 1 - .../src/abb_blend_process_service.cpp | 67 +- .../src/abb_blend_process_service_node.cpp | 2 +- .../src/blend_process_service.cpp | 21 +- .../src/keyence_process_service.cpp | 37 +- .../src/keyence_process_service_node.cpp | 2 +- godel_process_execution/src/process_utils.cpp | 9 +- godel_process_execution/src/process_utils.h | 2 +- .../get_boundary.h | 79 +- .../mesh_importer.h | 45 +- .../polygon_pts.hpp | 53 +- .../polygon_utils.h | 44 +- .../process_path.h | 21 +- .../process_path_generator.h | 97 +- .../process_pt.h | 62 +- .../process_transition.h | 101 +- .../godel_process_path_generation/utils.h | 123 +- .../src/mesh_importer.cpp | 315 +- .../src/polygon_utils.cpp | 95 +- .../src/process_path.cpp | 4 +- .../src/process_path_generator.cpp | 68 +- .../src/process_path_generator_node.cpp | 59 +- .../src/process_path_visualization_node.cpp | 93 +- .../src/process_transition.cpp | 5 +- .../test/test_MeshImporter.cpp | 131 +- .../test/test_ProcessPathGenerator.cpp | 16 +- .../test/test_common_functions.h | 119 +- .../test/test_polygon_utils.cpp | 33 +- .../godel_process_planning.h | 4 +- .../src/blend_process_planning.cpp | 79 +- godel_process_planning/src/common_utils.cpp | 89 +- godel_process_planning/src/common_utils.h | 195 +- .../src/godel_process_planning.cpp | 15 +- .../src/godel_process_planning_node.cpp | 12 +- .../src/keyence_process_planning.cpp | 74 +- .../src/trajectory_utils.cpp | 42 +- godel_process_planning/src/trajectory_utils.h | 32 +- .../abb_motion_ftp_downloader.h | 9 +- .../rapid_generator/rapid_data_structures.h | 20 +- .../include/rapid_generator/rapid_emitter.h | 80 +- .../rapid_generator/src/rapid_emitter.cpp | 87 +- .../src/abb_motion_ftp_downloader.cpp | 65 +- .../src/abb_motion_ftp_downloader_node.cpp | 3 +- .../abb/abb_file_suite/src/ftp_upload.cpp | 36 +- .../abb/abb_file_suite/src/ftp_upload.h | 3 - .../abb_irb2400_robot_model.h | 48 +- .../src/abb_irb2400_robot_model.cpp | 147 +- ...b2400_manipulator_ikfast_moveit_plugin.hpp | 803 +- .../abb_irb2400_manipulator_ikfast_solver.hpp | 5843 +++++---- .../ikfast.h | 384 +- .../src/plugin_init.cpp | 5 +- .../godel_scan_analysis/keyence_scan_server.h | 95 +- .../godel_scan_analysis/scan_algorithms.h | 29 +- .../scan_roughness_scoring.h | 43 +- .../godel_scan_analysis/scan_utilities.h | 26 +- .../src/godel_scan_analysis_node.cpp | 7 +- .../src/keyence_scan_server.cpp | 28 +- .../src/scan_roughness_scoring.cpp | 149 +- .../include/godel_simple_gui/blending_panel.h | 5 +- .../godel_simple_gui/blending_widget.h | 9 +- .../include/godel_simple_gui/gui_state.h | 3 - .../options/blend_tool_configuration.h | 6 +- .../godel_simple_gui/options/pose_widget.h | 20 +- .../options/robot_scan_configuration.h | 7 +- .../options/scan_tool_configuration.h | 8 +- .../options/surface_detection_configuration.h | 7 +- .../godel_simple_gui/options_submenu.h | 6 +- .../godel_simple_gui/parameter_window_base.h | 2 - .../godel_simple_gui/states/error_state.h | 2 - .../godel_simple_gui/states/executing_state.h | 2 - .../godel_simple_gui/states/planning_state.h | 2 - .../states/scan_teach_state.h | 1 - .../godel_simple_gui/states/scanning_state.h | 1 - .../states/simulating_state.h | 2 - .../states/surface_select_state.h | 1 - .../states/wait_for_execute_state.h | 2 - .../states/wait_to_execute_state.h | 2 - .../states/wait_to_simulate_state.h | 2 - godel_simple_gui/src/blending_panel.cpp | 3 +- godel_simple_gui/src/blending_widget.cpp | 38 +- .../src/options/blend_tool_configuration.cpp | 5 +- godel_simple_gui/src/options/pose_widget.cpp | 17 +- .../src/options/robot_scan_configuration.cpp | 36 +- .../src/options/scan_tool_configuration.cpp | 3 +- .../surface_detection_configuration.cpp | 23 +- godel_simple_gui/src/options_submenu.cpp | 23 +- .../src/parameter_window_base.cpp | 6 +- godel_simple_gui/src/states/error_state.cpp | 21 +- .../src/states/executing_state.cpp | 47 +- .../src/states/planning_state.cpp | 30 +- .../src/states/scan_teach_state.cpp | 16 +- .../src/states/scanning_state.cpp | 35 +- .../src/states/simulating_state.cpp | 38 +- .../src/states/surface_select_state.cpp | 11 +- .../src/states/wait_to_execute_state.cpp | 13 +- .../src/states/wait_to_simulate_state.cpp | 23 +- .../detection/surface_detection.h | 294 +- .../interactive/interactive_surface_server.h | 197 +- .../scan/profilimeter_scan.h | 10 +- .../godel_surface_detection/scan/robot_scan.h | 100 +- .../services/surface_blending_service.h | 62 +- .../services/trajectory_library.h | 1 - .../src/detection/surface_detection.cpp | 1316 +- .../interactive_surface_server.cpp | 890 +- .../src/nodes/point_cloud_generator_node.cpp | 468 +- .../src/nodes/point_cloud_publisher_node.cpp | 326 +- .../src/nodes/robot_scan_node.cpp | 287 +- .../src/nodes/surface_detection_node.cpp | 330 +- .../src/scan/profilimeter_scan.cpp | 298 +- .../src/scan/robot_scan.cpp | 627 +- .../blending_service_path_generation.cpp | 66 +- .../src/services/surface_blending_service.cpp | 308 +- .../src/services/trajectory_library.cpp | 2 +- .../src/simulator_service_node.cpp | 140 +- .../motoman_sia20d_robot_model.h | 28 +- .../src/motoman_sia20d_robot_model.cpp | 149 +- motoman_sia20d_descartes/src/plugin_init.cpp | 3 +- .../test/motoman_sia20d_descartes_plugin.cpp | 19 +- motoman_sia20d_descartes/test/utest.cpp | 2 +- .../config/ikfast_sia20d_manipulator.cpp | 10255 +++++++++------- .../ikfast.h | 385 +- ...ia20d_manipulator_ikfast_moveit_plugin.hpp | 819 +- ...toman_sia20d_manipulator_ikfast_solver.hpp | 10155 +++++++++------ .../src/plugin_init.cpp | 6 +- 152 files changed, 22607 insertions(+), 17405 deletions(-) diff --git a/godel_keyence_ljv_driver/include/godel_keyence_ljv_driver/Keyence_tcp_client.h b/godel_keyence_ljv_driver/include/godel_keyence_ljv_driver/Keyence_tcp_client.h index c9dd689c..853d85ed 100644 --- a/godel_keyence_ljv_driver/include/godel_keyence_ljv_driver/Keyence_tcp_client.h +++ b/godel_keyence_ljv_driver/include/godel_keyence_ljv_driver/Keyence_tcp_client.h @@ -39,41 +39,46 @@ #include - namespace industrial { namespace tcp_client { - /** * \brief Defines TCP client functions. * * public industrial::tcp_socket::TcpSocket */ -class Keyence_TcpClient : public industrial::tcp_client::TcpClient, public industrial::byte_array::ByteArray +class Keyence_TcpClient : public industrial::tcp_client::TcpClient, + public industrial::byte_array::ByteArray { public: - // Provides SimpleSerialize access to byte array internals - //friend class SimpleSerialize; + // friend class SimpleSerialize; /** * \brief Constructor */ - Keyence_TcpClient(){} + Keyence_TcpClient() {} /** * \brief Destructor */ - ~Keyence_TcpClient(){} + ~Keyence_TcpClient() {} - bool init(char *buff, int port_num){return industrial::tcp_client::TcpClient::init(buff,port_num);} - bool my_sendBytes(industrial::byte_array::ByteArray& buf){return sendBytes(buf);} - bool my_receiveBytes(industrial::byte_array::ByteArray& buf, industrial::shared_types::shared_int num_bytes){return receiveBytes(buf, num_bytes);} + bool init(char* buff, int port_num) + { + return industrial::tcp_client::TcpClient::init(buff, port_num); + } + bool my_sendBytes(industrial::byte_array::ByteArray& buf) { return sendBytes(buf); } + bool my_receiveBytes(industrial::byte_array::ByteArray& buf, + industrial::shared_types::shared_int num_bytes) + { + return receiveBytes(buf, num_bytes); + } }; -} //tcp_client -} //industrial +} // tcp_client +} // industrial #endif /* GODEL_KEYENCE_LJV_DRIVER_TCP_CLIENT_H */ diff --git a/godel_keyence_ljv_driver/src/driver.cpp b/godel_keyence_ljv_driver/src/driver.cpp index 06c2787d..e7669015 100644 --- a/godel_keyence_ljv_driver/src/driver.cpp +++ b/godel_keyence_ljv_driver/src/driver.cpp @@ -57,8 +57,6 @@ // pkg local includes #include "ljv7_rawdata.h" - - // keyence protocol / profile related defines #define KEYENCE_DEFAULT_TCP_PORT 24691 #define KEYENCE_DEFAULT_TCP_PORT_HS 24692 @@ -72,15 +70,13 @@ #define KEYENCE_INFINITE_DISTANCE_VALUE2 -524286 // values LJ Navigator uses for out-of-range points (in meters) -#define KEYENCE_INFINITE_DISTANCE_VALUE_SI -999.9990d / 1e3 +#define KEYENCE_INFINITE_DISTANCE_VALUE_SI -999.9990d / 1e3 #define KEYENCE_INFINITE_DISTANCE_VALUE_SI2 -999.9970d / 1e3 - // default values for parameters #define DEFAULT_SAMPLE_RATE 10.0 #define DEFAULT_FRAME_ID "sensor_optical_frame" - // error codes for receive_get_profile_response(..) #define RESP_ERR_OK 0 #define RESP_ERR_PFX_LEN 1 @@ -92,7 +88,6 @@ #define RESP_ERR_ABN_BODY_LEN 7 #define RESP_ERR_HDR_RET_CODE 8 - // local types typedef pcl::PointCloud point_cloud_t; @@ -111,37 +106,39 @@ typedef struct bool cnv_inf_pts; } config_t; - /** * Extract type at 'offset' into byte array pointed to by 'buf'. */ -template -static T extract_field(unsigned char* buf, uint32_t offset) { return ( *((T*) (buf+offset))); } - +template static T extract_field(unsigned char* buf, uint32_t offset) +{ + return (*((T*)(buf + offset))); +} // prototypes bool send_get_profile_request(industrial::tcp_client::Keyence_TcpClient& tcp_client); -int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_client, unsigned char* response_data, uint32_t* response_data_sz); -int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, keyence_profile_t& profile); -int keyence_profile_to_pc(keyence_profile_t& profile, point_cloud_t::Ptr msg, bool cnv_inf_pts, double scale_factor); - +int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_client, + unsigned char* response_data, uint32_t* response_data_sz); +int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, + keyence_profile_t& profile); +int keyence_profile_to_pc(keyence_profile_t& profile, point_cloud_t::Ptr msg, bool cnv_inf_pts, + double scale_factor); // TODO: refactor node to (a) proper clas(ses), avoid global variables config_t config_; -void dr_callback(godel_keyence_ljv_driver::KeyenceConfig &config, uint32_t level) +void dr_callback(godel_keyence_ljv_driver::KeyenceConfig& config, uint32_t level) { ROS_DEBUG("Reconfigure Request (scale_factor: %.2f; " - "cnv_inf_pts: %s)", config.scale_factor, config.cnv_inf_pts ? "true" : "false"); + "cnv_inf_pts: %s)", + config.scale_factor, config.cnv_inf_pts ? "true" : "false"); config_.pc_scale_factor = config.scale_factor; config_.cnv_inf_pts = config.cnv_inf_pts; } - int main(int argc, char** argv) { - ros::init (argc, argv, "keyence_lj_driver"); + ros::init(argc, argv, "keyence_lj_driver"); ros::NodeHandle nh, pnh("~"); // ros parameters @@ -185,8 +182,8 @@ int main(int argc, char** argv) pnh.param("sample_rate", sample_rate, DEFAULT_SAMPLE_RATE); pnh.param("frame_id", frame_id, DEFAULT_FRAME_ID); - ROS_INFO("Connecting to %s (TCP %d), expecting a single %s head", - sensor_host.c_str(), sensor_port, head_a_model.c_str()); + ROS_INFO("Connecting to %s (TCP %d), expecting a single %s head", sensor_host.c_str(), + sensor_port, head_a_model.c_str()); ROS_INFO("Scaling profiles %.2f times", config_.pc_scale_factor); ROS_INFO("Attempting to publish at %.2f Hz", sample_rate); @@ -195,7 +192,7 @@ int main(int argc, char** argv) ROS_INFO("Profile points at infinite distances published with Z: +Inf (REP-117)"); else ROS_INFO("Profile points at infinite distances published with Z: %.2f (m)", - KEYENCE_INFINITE_DISTANCE_VALUE_SI); + KEYENCE_INFINITE_DISTANCE_VALUE_SI); // setup point cloud message (we reuse single one) // TODO: this won't work with nodelets @@ -211,12 +208,11 @@ int main(int argc, char** argv) tcp_client.init(const_cast(sensor_host.c_str()), sensor_port); if (!tcp_client.makeConnect()) { - ROS_FATAL("Could not connect to controller at %s (TCP %d). Aborting", - sensor_host.c_str(), sensor_port); + ROS_FATAL("Could not connect to controller at %s (TCP %d). Aborting", sensor_host.c_str(), + sensor_port); return EX_IOERR; } - /** * Main loop: * @@ -228,7 +224,7 @@ int main(int argc, char** argv) */ int res = 0; ros::Rate sleeper(sample_rate); - while(ros::ok() /* && still connected*/) + while (ros::ok() /* && still connected*/) { // sleep? sleeper.sleep(); @@ -245,13 +241,13 @@ int main(int argc, char** argv) continue; } - if(!send_get_profile_request(tcp_client)) + if (!send_get_profile_request(tcp_client)) { ROS_FATAL("Sending GetProfile request failed. Aborting"); break; } - if((res = receive_get_profile_response(tcp_client, response_data, &response_data_sz)) < 0) + if ((res = receive_get_profile_response(tcp_client, response_data, &response_data_sz)) < 0) { if (res == -RESP_ERR_EAGAIN) { @@ -290,7 +286,6 @@ int main(int argc, char** argv) return EX_OK; } - bool send_get_profile_request(industrial::tcp_client::Keyence_TcpClient& tcp_client) { // TODO: lots of magic nrs (protocol constants) @@ -301,14 +296,14 @@ bool send_get_profile_request(industrial::tcp_client::Keyence_TcpClient& tcp_cli buf.load(0x00000020); // total pkt length: 32 bytes buf.load(0x00F00001); // it's a request & pkg version - buf.load( 0x0); // 'fixed as 0x00' + buf.load(0x0); // 'fixed as 0x00' buf.load(0x00000014); // body length: 20 bytes - buf.load( 0x42); // command code: get profile + buf.load(0x42); // command code: get profile - buf.load( 0x0); // profile bank: active surface (page 9, comm lib refman) - buf.load( 0x0); // profile pos : from current (page 9, comm lib refman) - buf.load( 0x1); // nr of profile (how many profiles do we want in a single reply) + buf.load(0x0); // profile bank: active surface (page 9, comm lib refman) + buf.load(0x0); // profile pos : from current (page 9, comm lib refman) + buf.load(0x1); // nr of profile (how many profiles do we want in a single reply) buf.load(0x00000101); // last bits: 'nr of demanded profile' & 'erase reading data' // 'nr of demanded profile' only valid/necessary when 'profile pos' // has been set to '2: specify position' @@ -320,11 +315,11 @@ bool send_get_profile_request(industrial::tcp_client::Keyence_TcpClient& tcp_cli return tcp_client.my_sendBytes(buf); } - /** * Returns the 'response data' part of a GetProfile reply */ -int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_client, unsigned char* response_data, uint32_t* response_data_sz) +int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_client, + unsigned char* response_data, uint32_t* response_data_sz) { // TODO: magic nr: large 'enough' const uint32_t buf_sz = 4096; @@ -336,7 +331,7 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ barray.init(); // first receive prefix.pkt_len, so we know how large pkt is - if(!tcp_client.my_receiveBytes(barray, sizeof(pkt_len))) + if (!tcp_client.my_receiveBytes(barray, sizeof(pkt_len))) { ROS_ERROR("Error receiving prefix.pkt_len. Aborting"); return -RESP_ERR_PFX_LEN; @@ -344,16 +339,15 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ ROS_DEBUG("raw pkt_len: 0x%X", *((uint32_t*)barray.getRawDataPtr())); - if(!barray.unload(&pkt_len, sizeof(pkt_len))) + if (!barray.unload(&pkt_len, sizeof(pkt_len))) { ROS_ERROR("Error unloading prefix.pkt_len. Aborting"); return -RESP_ERR_UNL_PFX_LEN; } - if(pkt_len >= buf_sz) + if (pkt_len >= buf_sz) { - ROS_ERROR("Incoming pkt too large for buffer (%d > %d). Aborting", - pkt_len, buf_sz); + ROS_ERROR("Incoming pkt too large for buffer (%d > %d). Aborting", pkt_len, buf_sz); return -RESP_ERR_PKT_BUFF_OVERFLOW; } @@ -365,7 +359,7 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ ROS_DEBUG("Reading %d bytes from socket", to_receive); // TODO: This is blocking, no way to timeout from this - if(!tcp_client.my_receiveBytes(barray, to_receive)) + if (!tcp_client.my_receiveBytes(barray, to_receive)) { ROS_ERROR("Error receiving GetProfile response part. Aborting"); return -RESP_ERR_RECV_PAYLOAD; @@ -379,22 +373,23 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ if (buf_ptr > pkt_len) { ROS_WARN("GetProfile response was larger than " - "expected (%d > %d)", buf_ptr, pkt_len); + "expected (%d > %d)", + buf_ptr, pkt_len); } // extract fields from raw pkt data - const uint8_t return_code = extract_field(buf, 4); + const uint8_t return_code = extract_field(buf, 4); const uint32_t body_length = extract_field(buf, 8); - ROS_DEBUG("GetProfile response with %d retcode. Body length: %d", - return_code, body_length); + ROS_DEBUG("GetProfile response with %d retcode. Body length: %d", return_code, body_length); // check reply, make sure we got what we expected // TODO: magic nr if (return_code != 0) { ROS_WARN("Received unexpected return code " - "from sensor for GetProfile: 0x%02X", return_code); + "from sensor for GetProfile: 0x%02X", + return_code); return -RESP_ERR_HDR_RET_CODE; } @@ -403,7 +398,7 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ { // 12 bytes of response header, +1 for the 'command code' byte // in the response body - const uint8_t body_return_code = extract_field(buf, 13); + const uint8_t body_return_code = extract_field(buf, 13); if (body_return_code == KEYENCE_RET_CODE_NO_DATA) { @@ -420,7 +415,8 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ if (body_length < 512 /*bytes*/) { ROS_WARN("Abnormal (for GetProfile) body length encountered " - "(%d bytes), ignoring response", body_length); + "(%d bytes), ignoring response", + body_length); return -RESP_ERR_ABN_BODY_LEN; } @@ -438,10 +434,10 @@ int receive_get_profile_response(industrial::tcp_client::Keyence_TcpClient& tcp_ return RESP_ERR_OK; } - // assumption: 'profile' already initialised, with 'points' array already alloc-ed // assumption2: single head, no compression -int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, keyence_profile_t& profile) +int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, + keyence_profile_t& profile) { /* Need: * - number of profile points @@ -467,10 +463,10 @@ int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, unsigned char* point_data = profile_data + point_data_offset; // get simple fields - profile.num_points = extract_field(profile_info, 0); - profile.data_unit = extract_field(profile_info, 2); - profile.x_start = extract_field (profile_info, 4); - profile.x_increment = extract_field (profile_info, 8); + profile.num_points = extract_field(profile_info, 0); + profile.data_unit = extract_field(profile_info, 2); + profile.x_start = extract_field(profile_info, 4); + profile.x_increment = extract_field(profile_info, 8); // check if (profile.points == NULL) @@ -486,18 +482,18 @@ int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, const uint32_t encoder_count = extract_field(profile_data, 8); ROS_DEBUG("Unpacking profile %d. Nr of points: %d; data unit: %d; X Start: %d; X Incr: %d", - trigger_count, profile.num_points, profile.data_unit, profile.x_start, profile.x_increment); + trigger_count, profile.num_points, profile.data_unit, profile.x_start, + profile.x_increment); // unpack profile points // TODO: hard coded point_data sz (should calculate from pointers) const uint32_t point_data_sz = 2000; // bytes - int res = ljv7_unpack_profile_data(point_data, point_data_sz, - profile.num_points, profile.points, (profile.num_points * sizeof(profile_point_t))); + int res = ljv7_unpack_profile_data(point_data, point_data_sz, profile.num_points, profile.points, + (profile.num_points * sizeof(profile_point_t))); if (res != 0) { - ROS_WARN("Error unpacking profile data (err: %d). Ignoring profile %d", - res, trigger_count); + ROS_WARN("Error unpacking profile data (err: %d). Ignoring profile %d", res, trigger_count); return res; } @@ -505,13 +501,13 @@ int unpack_response_command_data(unsigned char* cmd_data, uint32_t cmd_data_sz, return 0; } - // assumption: 'msg' is a properly setup (and empty) PointCloud msg -int keyence_profile_to_pc(keyence_profile_t& profile, point_cloud_t::Ptr msg, bool cnv_inf_pts, double scale_factor) +int keyence_profile_to_pc(keyence_profile_t& profile, point_cloud_t::Ptr msg, bool cnv_inf_pts, + double scale_factor) { // TODO: get proper timestamp from somewhere // pcl header stamps are in microseconds - msg->header.stamp = ros::Time::now().toNSec()/1e3; + msg->header.stamp = ros::Time::now().toNSec() / 1e3; msg->width = profile.num_points; const double x_start = (profile.x_start * KEYENCE_PDEPTH_UNIT); @@ -528,7 +524,7 @@ int keyence_profile_to_pc(keyence_profile_t& profile, point_cloud_t::Ptr msg, bo // filter out 'infinite distance' points // REP-117: http://www.ros.org/reps/rep-0117.html // "out of range detections will be represented by +Inf." - if(profile.points[i] == KEYENCE_INFINITE_DISTANCE_VALUE) + if (profile.points[i] == KEYENCE_INFINITE_DISTANCE_VALUE) { if (cnv_inf_pts) z = std::numeric_limits::infinity(); @@ -538,7 +534,7 @@ int keyence_profile_to_pc(keyence_profile_t& profile, point_cloud_t::Ptr msg, bo // device returns two different values that are supposed to be interpreted // as out-of-range or 'infinite'. This is the second - if(profile.points[i] == KEYENCE_INFINITE_DISTANCE_VALUE2) + if (profile.points[i] == KEYENCE_INFINITE_DISTANCE_VALUE2) { if (cnv_inf_pts) z = std::numeric_limits::infinity(); diff --git a/godel_keyence_ljv_driver/src/ljv7_rawdata.cpp b/godel_keyence_ljv_driver/src/ljv7_rawdata.cpp index f74bb5ac..000e1a4c 100644 --- a/godel_keyence_ljv_driver/src/ljv7_rawdata.cpp +++ b/godel_keyence_ljv_driver/src/ljv7_rawdata.cpp @@ -36,16 +36,16 @@ #include #include - -int ljv7_unpack_profile_data(unsigned char* src, uint32_t src_sz, uint32_t num_pp, profile_point_t* dst, uint32_t dst_sz) +int ljv7_unpack_profile_data(unsigned char* src, uint32_t src_sz, uint32_t num_pp, + profile_point_t* dst, uint32_t dst_sz) { // constants from protocol documentation const uint32_t bpp = 20; // bits per profile point - const uint32_t ppr = 8; // profiles per 'row' + const uint32_t ppr = 8; // profiles per 'row' const uint32_t bpb = sizeof(char) * CHAR_BIT; // bits per byte - const uint32_t bpr = (bpp * ppr) / bpb; // bytes per 'row' - const uint32_t ipr = bpr / sizeof(int32_t); // ints per 'row' + const uint32_t bpr = (bpp * ppr) / bpb; // bytes per 'row' + const uint32_t ipr = bpr / sizeof(int32_t); // ints per 'row' unsigned int i = 0, j = 0; // make sure source buffer is multiple of 'bytes per row' bytes @@ -67,24 +67,24 @@ int ljv7_unpack_profile_data(unsigned char* src, uint32_t src_sz, uint32_t num_p return -3; } - uint32_t* ints_ptr = (uint32_t*) src; + uint32_t* ints_ptr = (uint32_t*)src; // from: https://graphics.stanford.edu/~seander/bithacks.html#FixedSignExtend const int32_t m = 1U << (bpp - 1); - for (i = 0, j = 0; j < num_pp; i+=ipr, j+=ppr) + for (i = 0, j = 0; j < num_pp; i += ipr, j += ppr) { // unpack 'ppr' points at a time (which needs 'ipr' int32's) - dst[j ] = ((ints_ptr[i ] & 0x000FFFFF)); - dst[j + 1] = ((ints_ptr[i ] & 0xFFF00000) >> 20) | ((ints_ptr[i+1] & 0x000000FF) << 12); - dst[j + 2] = ((ints_ptr[i+1] & 0x0FFFFF00) >> 8); - dst[j + 3] = ((ints_ptr[i+1] & 0xF0000000) >> 28) | ((ints_ptr[i+2] & 0x0000FFFF) << 4); + dst[j] = ((ints_ptr[i] & 0x000FFFFF)); + dst[j + 1] = ((ints_ptr[i] & 0xFFF00000) >> 20) | ((ints_ptr[i + 1] & 0x000000FF) << 12); + dst[j + 2] = ((ints_ptr[i + 1] & 0x0FFFFF00) >> 8); + dst[j + 3] = ((ints_ptr[i + 1] & 0xF0000000) >> 28) | ((ints_ptr[i + 2] & 0x0000FFFF) << 4); - dst[j + 4] = ((ints_ptr[i+2] & 0xFFFF0000) >> 16) | ((ints_ptr[i+3] & 0x0000000F) << 16); - dst[j + 5] = ((ints_ptr[i+3] & 0x00FFFFF0) >> 4); - dst[j + 6] = ((ints_ptr[i+3] & 0xFF000000) >> 24) | ((ints_ptr[i+4] & 0x00000FFF) << 8); - dst[j + 7] = ((ints_ptr[i+4] & 0xFFFFF000) >> 12); + dst[j + 4] = ((ints_ptr[i + 2] & 0xFFFF0000) >> 16) | ((ints_ptr[i + 3] & 0x0000000F) << 16); + dst[j + 5] = ((ints_ptr[i + 3] & 0x00FFFFF0) >> 4); + dst[j + 6] = ((ints_ptr[i + 3] & 0xFF000000) >> 24) | ((ints_ptr[i + 4] & 0x00000FFF) << 8); + dst[j + 7] = ((ints_ptr[i + 4] & 0xFFFFF000) >> 12); // todo: rework this - dst[j ] = (dst[j] ^ m) - m; + dst[j] = (dst[j] ^ m) - m; dst[j + 1] = (dst[j + 1] ^ m) - m; dst[j + 2] = (dst[j + 2] ^ m) - m; dst[j + 3] = (dst[j + 3] ^ m) - m; diff --git a/godel_keyence_ljv_driver/src/ljv7_rawdata.h b/godel_keyence_ljv_driver/src/ljv7_rawdata.h index 5d7777d8..6ec11992 100644 --- a/godel_keyence_ljv_driver/src/ljv7_rawdata.h +++ b/godel_keyence_ljv_driver/src/ljv7_rawdata.h @@ -31,15 +31,12 @@ #ifndef __KEYENCE_DRIVER_LJV7_RAWDATA_H__ #define __KEYENCE_DRIVER_LJV7_RAWDATA_H__ - #include #include - typedef int32_t profile_point_t; - -int ljv7_unpack_profile_data(unsigned char* src, uint32_t src_sz, uint32_t num_pp, profile_point_t* dst, uint32_t dst_sz); - +int ljv7_unpack_profile_data(unsigned char* src, uint32_t src_sz, uint32_t num_pp, + profile_point_t* dst, uint32_t dst_sz); #endif // __KEYENCE_DRIVER_LJV7_RAWDATA_H__ diff --git a/godel_param_helpers/include/godel_param_helpers/godel_param_helpers.h b/godel_param_helpers/include/godel_param_helpers/godel_param_helpers.h index 87b58976..d5c89df1 100644 --- a/godel_param_helpers/include/godel_param_helpers/godel_param_helpers.h +++ b/godel_param_helpers/include/godel_param_helpers/godel_param_helpers.h @@ -8,17 +8,16 @@ namespace godel_param_helpers { // Write a message to disk -template -inline bool toFile(const std::string& path, const T& msg) +template inline bool toFile(const std::string& path, const T& msg) { namespace ser = ros::serialization; uint32_t serialize_size = ser::serializationLength(msg); - boost::shared_array buffer (new uint8_t[serialize_size]); + boost::shared_array buffer(new uint8_t[serialize_size]); ser::OStream stream(buffer.get(), serialize_size); ser::serialize(stream, msg); - std::ofstream file (path.c_str(), std::ios::out | std::ios::binary); + std::ofstream file(path.c_str(), std::ios::out | std::ios::binary); if (!file) { return false; @@ -31,12 +30,11 @@ inline bool toFile(const std::string& path, const T& msg) } // Restore a message from disk -template -inline bool fromFile(const std::string& path, T& msg) +template inline bool fromFile(const std::string& path, T& msg) { namespace ser = ros::serialization; - std::ifstream ifs (path.c_str(), std::ios::in | std::ios::binary); + std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary); if (!ifs) { return false; @@ -49,17 +47,16 @@ inline bool fromFile(const std::string& path, T& msg) uint32_t file_size = end - begin; - boost::shared_array ibuffer (new uint8_t[file_size]); + boost::shared_array ibuffer(new uint8_t[file_size]); ifs.read((char*)ibuffer.get(), file_size); - ser::IStream istream (ibuffer.get(), file_size); + ser::IStream istream(ibuffer.get(), file_size); ser::deserialize(istream, msg); return true; } // for loading parameters from server // parameter loading helper -template -inline bool loadParam(ros::NodeHandle& nh, const std::string& name, T& value) +template inline bool loadParam(ros::NodeHandle& nh, const std::string& name, T& value) { if (!nh.getParam(name, value)) { diff --git a/godel_path_execution/include/godel_path_execution/path_execution_service.h b/godel_path_execution/include/godel_path_execution/path_execution_service.h index e30afe69..1d55c2bc 100644 --- a/godel_path_execution/include/godel_path_execution/path_execution_service.h +++ b/godel_path_execution/include/godel_path_execution/path_execution_service.h @@ -6,7 +6,6 @@ #include #include - namespace godel_path_execution { @@ -22,12 +21,12 @@ class PathExecutionService */ bool executionCallback(godel_msgs::TrajectoryExecution::Request& req, godel_msgs::TrajectoryExecution::Response& res); + private: ros::ServiceServer server_; actionlib::SimpleActionClient ac_; std::string name_; }; - } #endif // PATH_EXECUTION_SERVICE_H diff --git a/godel_path_execution/src/path_execution_service.cpp b/godel_path_execution/src/path_execution_service.cpp index c1527760..a0382954 100644 --- a/godel_path_execution/src/path_execution_service.cpp +++ b/godel_path_execution/src/path_execution_service.cpp @@ -2,19 +2,18 @@ #include const static std::string ACTION_SERVER_NAME = "joint_trajectory_action"; -const static double ACTION_EXTRA_WAIT_RATIO = 0.2; // 20% past end of trajectory +const static double ACTION_EXTRA_WAIT_RATIO = 0.2; // 20% past end of trajectory const static double ACTION_SERVICE_WAIT_TIME = 30.0; // seconds const static char* const ACTION_CONNECTION_FAILED_MSG = "Could not connect to action server."; const static std::string THIS_SERVICE_NAME = "path_execution"; godel_path_execution::PathExecutionService::PathExecutionService(ros::NodeHandle& nh) - : ac_(ACTION_SERVER_NAME, true) -{ - server_ = nh.advertiseService - (THIS_SERVICE_NAME, &godel_path_execution::PathExecutionService::executionCallback, this); + : ac_(ACTION_SERVER_NAME, true) +{ + server_ = nh.advertiseService( + THIS_SERVICE_NAME, &godel_path_execution::PathExecutionService::executionCallback, this); // Attempt to connect to the motion action service if (!ac_.waitForServer(ros::Duration(ACTION_SERVICE_WAIT_TIME))) @@ -24,8 +23,8 @@ godel_path_execution::PathExecutionService::PathExecutionService(ros::NodeHandle } } -bool godel_path_execution::PathExecutionService::executionCallback(godel_msgs::TrajectoryExecution::Request& req, - godel_msgs::TrajectoryExecution::Response& res) +bool godel_path_execution::PathExecutionService::executionCallback( + godel_msgs::TrajectoryExecution::Request& req, godel_msgs::TrajectoryExecution::Response& res) { // Check preconditions if (!ac_.isServerConnected()) @@ -34,7 +33,7 @@ bool godel_path_execution::PathExecutionService::executionCallback(godel_msgs::T return false; } - if (req.trajectory.points.empty()) + if (req.trajectory.points.empty()) { ROS_WARN_STREAM("Trajectory Execution Service recieved an empty trajectory."); return true; @@ -47,7 +46,8 @@ bool godel_path_execution::PathExecutionService::executionCallback(godel_msgs::T if (req.wait_for_execution) { - ros::Duration extra_wait = goal.trajectory.points.back().time_from_start * ACTION_EXTRA_WAIT_RATIO; + ros::Duration extra_wait = + goal.trajectory.points.back().time_from_start * ACTION_EXTRA_WAIT_RATIO; if (ac_.waitForResult(goal.trajectory.points.back().time_from_start + extra_wait)) { return ac_.getState().state_ == ac_.getState().SUCCEEDED; diff --git a/godel_plugins/include/godel_plugins/rqt_plugins/robot_blending_plugin.h b/godel_plugins/include/godel_plugins/rqt_plugins/robot_blending_plugin.h index 6feb3328..831bb64f 100644 --- a/godel_plugins/include/godel_plugins/rqt_plugins/robot_blending_plugin.h +++ b/godel_plugins/include/godel_plugins/rqt_plugins/robot_blending_plugin.h @@ -1,17 +1,17 @@ /* - Copyright Feb 10, 2014 Southwest Research Institute + Copyright Feb 10, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #ifndef ROBOT_BLENDING_PLUGIN_H_ @@ -22,31 +22,33 @@ #include #include -namespace godel_plugins{ namespace rqt_plugins{ +namespace godel_plugins +{ +namespace rqt_plugins +{ class RobotBlendingPlugin : public rqt_gui_cpp::Plugin { public: - static const std::string QOBJECT_NAME; + static const std::string QOBJECT_NAME; - - Q_OBJECT + Q_OBJECT public: - RobotBlendingPlugin(); - virtual ~RobotBlendingPlugin(); + RobotBlendingPlugin(); + virtual ~RobotBlendingPlugin(); - virtual void initPlugin(qt_gui_cpp::PluginContext& context); - virtual void shutdownPlugin(); - virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, - qt_gui_cpp::Settings& instance_settings) const; - virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, - const qt_gui_cpp::Settings& instance_settings); + virtual void initPlugin(qt_gui_cpp::PluginContext& context); + virtual void shutdownPlugin(); + virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, + qt_gui_cpp::Settings& instance_settings) const; + virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, + const qt_gui_cpp::Settings& instance_settings); private: - QWidget* widget_; + QWidget* widget_; }; - -}} +} +} #endif /* ROBOT_BLENDING_PLUGIN_H_ */ diff --git a/godel_plugins/include/godel_plugins/rviz_panels/robot_blending_panel.h b/godel_plugins/include/godel_plugins/rviz_panels/robot_blending_panel.h index 580e9917..db474ab0 100644 --- a/godel_plugins/include/godel_plugins/rviz_panels/robot_blending_panel.h +++ b/godel_plugins/include/godel_plugins/rviz_panels/robot_blending_panel.h @@ -1,17 +1,17 @@ /* - Copyright Mar 13, 2014 Southwest Research Institute + Copyright Mar 13, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #ifndef ROBOT_BLENDING_PANEL_H_ @@ -21,26 +21,28 @@ #include #include -namespace godel_plugins { -namespace rviz_panels { +namespace godel_plugins +{ +namespace rviz_panels +{ -class RobotBlendingPanel: public rviz::Panel { -Q_OBJECT +class RobotBlendingPanel : public rviz::Panel +{ + Q_OBJECT public: - RobotBlendingPanel(QWidget* parent = 0); - virtual ~RobotBlendingPanel(); + RobotBlendingPanel(QWidget* parent = 0); + virtual ~RobotBlendingPanel(); - virtual void onInitialize(); + virtual void onInitialize(); protected Q_SLOTS: - // rviz::Panel virtual functions - virtual void load(const rviz::Config& config); - virtual void save(rviz::Config config) const; + // rviz::Panel virtual functions + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; protected: - - QWidget *widget_; + QWidget* widget_; }; } /* namespace rviz_panels */ diff --git a/godel_plugins/include/godel_plugins/widgets/blend_tool_param_window.h b/godel_plugins/include/godel_plugins/widgets/blend_tool_param_window.h index 45337db1..22285112 100644 --- a/godel_plugins/include/godel_plugins/widgets/blend_tool_param_window.h +++ b/godel_plugins/include/godel_plugins/widgets/blend_tool_param_window.h @@ -5,7 +5,6 @@ #include #include - namespace godel_plugins { @@ -25,7 +24,6 @@ class BlendingPlanConfigWidget : public ParameterWindowBase godel_msgs::BlendingPlanParameters params_; Ui::BlendToolParamWindow ui_; }; - } #endif // BLEND_TOOL_PARAM_WINDOW_H diff --git a/godel_plugins/include/godel_plugins/widgets/parameter_window_base.h b/godel_plugins/include/godel_plugins/widgets/parameter_window_base.h index e4332901..f49f279e 100644 --- a/godel_plugins/include/godel_plugins/widgets/parameter_window_base.h +++ b/godel_plugins/include/godel_plugins/widgets/parameter_window_base.h @@ -39,8 +39,6 @@ protected Q_SLOTS: virtual void cancel_changes_handler(); virtual void save_changes_handler(); }; - } #endif // PARAMETER_WINDOW_BASE_H - diff --git a/godel_plugins/include/godel_plugins/widgets/robot_blending_widget.h b/godel_plugins/include/godel_plugins/widgets/robot_blending_widget.h index 462d8ad0..c7d2d1d7 100644 --- a/godel_plugins/include/godel_plugins/widgets/robot_blending_widget.h +++ b/godel_plugins/include/godel_plugins/widgets/robot_blending_widget.h @@ -1,17 +1,17 @@ /* - Copyright Mar 13, 2014 Southwest Research Institute + Copyright Mar 13, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #ifndef ROBOT_BLENDING_WIDGET_H_ @@ -47,137 +47,122 @@ #define RAD2DEG(x) ((x)*57.29578) #endif - namespace godel_plugins { -namespace widgets { +namespace widgets +{ const std::string SURFACE_DETECTION_SERVICE = "surface_detection"; const std::string SURFACE_BLENDING_PARAMETERS_SERVICE = "surface_blending_parameters"; const std::string SELECT_SURFACE_SERVICE = "select_surface"; -const std::string PROCESS_PATH_SERVICE="process_path"; +const std::string PROCESS_PATH_SERVICE = "process_path"; const std::string SELECTED_SURFACES_CHANGED_TOPIC = "selected_surfaces_changed"; const std::string GET_AVAILABLE_MOTION_PLANS_SERVICE = "get_available_motion_plans"; const std::string SELECT_MOTION_PLAN_SERVICE = "select_motion_plan"; const std::string LOAD_SAVE_MOTION_PLAN_SERVICE = "load_save_motion_plan"; const std::string RENAME_SURFACE_SERVICE = "rename_surface"; - -class RobotBlendingWidget: public QWidget +class RobotBlendingWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: - RobotBlendingWidget(std::string ns=""); - virtual ~RobotBlendingWidget(); + RobotBlendingWidget(std::string ns = ""); + virtual ~RobotBlendingWidget(); - std::string get_name() - { - return "RobotBlending"; - } + std::string get_name() { return "RobotBlending"; } - int width() - { - return ui_.TabWidget->width(); - } + int width() { return ui_.TabWidget->width(); } - int height() - { - return ui_.TabWidget->height(); - } + int height() { return ui_.TabWidget->height(); } - void emit_signal_selection_change() - { - Q_EMIT selection_changed(); - } + void emit_signal_selection_change() { Q_EMIT selection_changed(); } Q_SIGNALS: - void selection_changed(); - void surface_detection_started(); - void surface_detection_completed(); - void connect_started(); - void connect_completed(); + void selection_changed(); + void surface_detection_started(); + void surface_detection_completed(); + void connect_started(); + void connect_completed(); protected: - - void init(); - void send_scan_and_find_request(); - void send_find_surface_request(); - void save_robot_scan_parameters(); - bool call_select_surface_service(godel_msgs::SelectSurface::Request &req); - bool call_surface_detection_service(godel_msgs::SurfaceDetection& s); - void selected_surface_changed_callback(godel_msgs::SelectedSurfacesChangedConstPtr msg); - void select_motion_plan(const std::string& name, bool simulate); - void request_available_motions(std::vector& plans); - void request_load_save_motions(const std::string& path, bool isLoad); - void update_motion_plan_list(const std::vector& names); + void init(); + void send_scan_and_find_request(); + void send_find_surface_request(); + void save_robot_scan_parameters(); + bool call_select_surface_service(godel_msgs::SelectSurface::Request& req); + bool call_surface_detection_service(godel_msgs::SurfaceDetection& s); + void selected_surface_changed_callback(godel_msgs::SelectedSurfacesChangedConstPtr msg); + void select_motion_plan(const std::string& name, bool simulate); + void request_available_motions(std::vector& plans); + void request_load_save_motions(const std::string& path, bool isLoad); + void update_motion_plan_list(const std::vector& names); protected Q_SLOTS: - void scan_button_handler(); - void find_surface_button_handler(); - void update_handler(); - void connect_to_services(); - void increase_tab_index_handler(); - void decrease_tab_index_handler(); - void selection_changed_handler(); - void select_all_handler(); - void deselect_all_handler(); - void hide_all_handler(); - void show_all_handler(); - void scan_options_click_handler(); - - void blend_options_click_handler(); - void scan_plan_options_click_handler(); - - void surface_options_click_handler(); - void robot_scan_params_changed_handler(); - void surface_detect_params_changed_handler(); - void preview_path_handler(); - void surface_detection_started_handler(); - void surface_detection_completed_handler(); - void connect_started_handler(); - void connect_completed_handler(); - void generate_process_path_handler(); - void request_save_parameters(); - - void simulate_motion_plan_handler(); - void execute_motion_plan_handler(); - - void save_motion_plan_handler(); - void load_motion_plan_handler(); - - void handle_surface_rename(QListWidgetItem* item); + void scan_button_handler(); + void find_surface_button_handler(); + void update_handler(); + void connect_to_services(); + void increase_tab_index_handler(); + void decrease_tab_index_handler(); + void selection_changed_handler(); + void select_all_handler(); + void deselect_all_handler(); + void hide_all_handler(); + void show_all_handler(); + void scan_options_click_handler(); + + void blend_options_click_handler(); + void scan_plan_options_click_handler(); + + void surface_options_click_handler(); + void robot_scan_params_changed_handler(); + void surface_detect_params_changed_handler(); + void preview_path_handler(); + void surface_detection_started_handler(); + void surface_detection_completed_handler(); + void connect_started_handler(); + void connect_completed_handler(); + void generate_process_path_handler(); + void request_save_parameters(); + + void simulate_motion_plan_handler(); + void execute_motion_plan_handler(); + + void save_motion_plan_handler(); + void load_motion_plan_handler(); + + void handle_surface_rename(QListWidgetItem* item); protected: - Ui::RobotBlendingWidget ui_; - RobotScanConfigWidget *robot_scan_config_window_; - BlendingPlanConfigWidget *robot_blend_config_window_; - SurfaceDetectionConfigWidget *surface_detect_config_window_; - ScanPlanConfigWidget *scan_plan_config_window_; - - - ros::ServiceClient surface_detection_client_; - ros::ServiceClient select_surface_client_; - ros::ServiceClient process_plan_client_; - ros::ServiceClient surface_blending_parameters_client_; - ros::ServiceClient get_motion_plans_client_; - ros::ServiceClient select_motion_plan_client_; - ros::ServiceClient load_save_motion_plan_client_; - ros::ServiceClient rename_surface_client_; - ros::Subscriber selected_surfaces_subs_; - - std::string param_ns_; - godel_msgs::RobotScanParameters robot_scan_parameters_; - godel_msgs::SurfaceDetectionParameters surf_detect_parameters_; - godel_msgs::BlendingPlanParameters blending_plan_parameters_; - godel_msgs::ScanPlanParameters scan_plan_parameters_; - godel_msgs::SurfaceDetection::Response latest_result_; - godel_msgs::SurfaceDetection::Request latest_request_; - godel_msgs::SelectedSurfacesChanged selected_surfaces_msg_; - - // service results - bool surface_detection_op_succeeded_; - std::string surface_detection_op_message_; + Ui::RobotBlendingWidget ui_; + RobotScanConfigWidget* robot_scan_config_window_; + BlendingPlanConfigWidget* robot_blend_config_window_; + SurfaceDetectionConfigWidget* surface_detect_config_window_; + ScanPlanConfigWidget* scan_plan_config_window_; + + ros::ServiceClient surface_detection_client_; + ros::ServiceClient select_surface_client_; + ros::ServiceClient process_plan_client_; + ros::ServiceClient surface_blending_parameters_client_; + ros::ServiceClient get_motion_plans_client_; + ros::ServiceClient select_motion_plan_client_; + ros::ServiceClient load_save_motion_plan_client_; + ros::ServiceClient rename_surface_client_; + ros::Subscriber selected_surfaces_subs_; + + std::string param_ns_; + godel_msgs::RobotScanParameters robot_scan_parameters_; + godel_msgs::SurfaceDetectionParameters surf_detect_parameters_; + godel_msgs::BlendingPlanParameters blending_plan_parameters_; + godel_msgs::ScanPlanParameters scan_plan_parameters_; + godel_msgs::SurfaceDetection::Response latest_result_; + godel_msgs::SurfaceDetection::Request latest_request_; + godel_msgs::SelectedSurfacesChanged selected_surfaces_msg_; + + // service results + bool surface_detection_op_succeeded_; + std::string surface_detection_op_message_; }; } /* namespace widgets */ diff --git a/godel_plugins/include/godel_plugins/widgets/robot_scan_configuration.h b/godel_plugins/include/godel_plugins/widgets/robot_scan_configuration.h index 4c77adf3..6f5bebc6 100644 --- a/godel_plugins/include/godel_plugins/widgets/robot_scan_configuration.h +++ b/godel_plugins/include/godel_plugins/widgets/robot_scan_configuration.h @@ -15,18 +15,18 @@ namespace godel_plugins /** * @brief The PoseWidget class */ -class PoseWidget: public QWidget +class PoseWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: - PoseWidget(QWidget *parent = NULL); + PoseWidget(QWidget* parent = NULL); - void set_values(const geometry_msgs::Pose& p); - void set_values(const tf::Transform &t); - tf::Transform get_values(); + void set_values(const geometry_msgs::Pose& p); + void set_values(const tf::Transform& t); + tf::Transform get_values(); protected: - Ui::PoseWidget ui_; + Ui::PoseWidget ui_; }; /** @@ -46,12 +46,10 @@ class RobotScanConfigWidget : public ParameterWindowBase virtual void update_internal_values(); godel_msgs::RobotScanParameters params_; - PoseWidget *world_to_obj_pose_widget_; - PoseWidget *tcp_to_cam_pose_widget_; + PoseWidget* world_to_obj_pose_widget_; + PoseWidget* tcp_to_cam_pose_widget_; Ui::RobotScanConfigWindow ui_; }; - } #endif // ROBOT_SCAN_CONFIGURATION_H - diff --git a/godel_plugins/include/godel_plugins/widgets/scan_tool_configuration_window.h b/godel_plugins/include/godel_plugins/widgets/scan_tool_configuration_window.h index 3272b9d8..3d8bc4b9 100644 --- a/godel_plugins/include/godel_plugins/widgets/scan_tool_configuration_window.h +++ b/godel_plugins/include/godel_plugins/widgets/scan_tool_configuration_window.h @@ -5,7 +5,6 @@ #include #include - namespace godel_plugins { @@ -25,7 +24,6 @@ class ScanPlanConfigWidget : public ParameterWindowBase godel_msgs::ScanPlanParameters params_; Ui::ScanToolConfigurationWindow ui_; }; - } #endif // SCAN_TOOL_CONFIGURATION_WINDOW_H diff --git a/godel_plugins/include/godel_plugins/widgets/surface_detection_configuration.h b/godel_plugins/include/godel_plugins/widgets/surface_detection_configuration.h index 46773c32..2d92e9fa 100644 --- a/godel_plugins/include/godel_plugins/widgets/surface_detection_configuration.h +++ b/godel_plugins/include/godel_plugins/widgets/surface_detection_configuration.h @@ -5,7 +5,6 @@ #include #include - namespace godel_plugins { @@ -25,8 +24,6 @@ class SurfaceDetectionConfigWidget : public ParameterWindowBase godel_msgs::SurfaceDetectionParameters params_; Ui::SurfaceDetectionConfigWindow ui_; }; - } #endif // SURFACE_DETECTION_CONFIGURATION_H - diff --git a/godel_plugins/src/rqt_plugins/robot_blending_plugin.cpp b/godel_plugins/src/rqt_plugins/robot_blending_plugin.cpp index ceb3c43d..2ef92a86 100644 --- a/godel_plugins/src/rqt_plugins/robot_blending_plugin.cpp +++ b/godel_plugins/src/rqt_plugins/robot_blending_plugin.cpp @@ -1,61 +1,57 @@ /* - Copyright Feb 10, 2014 Southwest Research Institute + Copyright Feb 10, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include #include -namespace godel_plugins{ namespace rqt_plugins{ +namespace godel_plugins +{ +namespace rqt_plugins +{ const std::string RobotBlendingPlugin::QOBJECT_NAME = "RobotBlending"; -RobotBlendingPlugin::RobotBlendingPlugin(): - rqt_gui_cpp::Plugin(), - widget_(0) +RobotBlendingPlugin::RobotBlendingPlugin() : rqt_gui_cpp::Plugin(), widget_(0) { - setObjectName(QString::fromStdString(QOBJECT_NAME)); + setObjectName(QString::fromStdString(QOBJECT_NAME)); } -RobotBlendingPlugin::~RobotBlendingPlugin() { - // TODO Auto-generated destructor stub +RobotBlendingPlugin::~RobotBlendingPlugin() +{ + // TODO Auto-generated destructor stub } void RobotBlendingPlugin::initPlugin(qt_gui_cpp::PluginContext& context) { - widget_ = new widgets::RobotBlendingWidget("rqt"); - context.addWidget(widget_); + widget_ = new widgets::RobotBlendingWidget("rqt"); + context.addWidget(widget_); } -void RobotBlendingPlugin::shutdownPlugin() -{ - -} +void RobotBlendingPlugin::shutdownPlugin() {} void RobotBlendingPlugin::saveSettings(qt_gui_cpp::Settings& plugin_settings, - qt_gui_cpp::Settings& instance_settings) const + qt_gui_cpp::Settings& instance_settings) const { - } void RobotBlendingPlugin::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, - const qt_gui_cpp::Settings& instance_settings) + const qt_gui_cpp::Settings& instance_settings) { - } - -}} +} +} PLUGINLIB_EXPORT_CLASS(godel_plugins::rqt_plugins::RobotBlendingPlugin, rqt_gui_cpp::Plugin) - diff --git a/godel_plugins/src/rviz_panels/robot_blending_panel.cpp b/godel_plugins/src/rviz_panels/robot_blending_panel.cpp index 15ec1b66..96f26f38 100644 --- a/godel_plugins/src/rviz_panels/robot_blending_panel.cpp +++ b/godel_plugins/src/rviz_panels/robot_blending_panel.cpp @@ -1,67 +1,64 @@ /* - Copyright Mar 13, 2014 Southwest Research Institute + Copyright Mar 13, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include "godel_plugins/rviz_panels/robot_blending_panel.h" -namespace godel_plugins { -namespace rviz_panels { - -RobotBlendingPanel::RobotBlendingPanel(QWidget *parent): - rviz::Panel(parent) - { - // TODO Auto-generated constructor stub -} - -RobotBlendingPanel::~RobotBlendingPanel() +namespace godel_plugins +{ +namespace rviz_panels { +RobotBlendingPanel::RobotBlendingPanel(QWidget* parent) : rviz::Panel(parent) +{ + // TODO Auto-generated constructor stub } +RobotBlendingPanel::~RobotBlendingPanel() {} + void RobotBlendingPanel::onInitialize() { - // creating main layout - ROS_INFO_STREAM("Initializing RobotBlendingPanel"); - widget_ = new widgets::RobotBlendingWidget("~"); - this->parentWidget()->resize(widget_->width(),widget_->height()); - QHBoxLayout* main_layout = new QHBoxLayout(this); - main_layout->addWidget(widget_); - + // creating main layout + ROS_INFO_STREAM("Initializing RobotBlendingPanel"); + widget_ = new widgets::RobotBlendingWidget("~"); + this->parentWidget()->resize(widget_->width(), widget_->height()); + QHBoxLayout* main_layout = new QHBoxLayout(this); + main_layout->addWidget(widget_); } void RobotBlendingPanel::load(const rviz::Config& config) { - rviz::Panel::load(config); - QString text_entry; - ROS_INFO_STREAM("rviz RobotBlendingPanel reading config file"); - if(config.mapGetString("TextEntry",&text_entry)) - { - //ROS_INFO_STREAM("Loaded TextEntry with value: "< -PLUGINLIB_EXPORT_CLASS(godel_plugins::rviz_panels::RobotBlendingPanel,rviz::Panel ) +PLUGINLIB_EXPORT_CLASS(godel_plugins::rviz_panels::RobotBlendingPanel, rviz::Panel) diff --git a/godel_plugins/src/widgets/blend_tool_param_window.cpp b/godel_plugins/src/widgets/blend_tool_param_window.cpp index a2223af9..df15ffb7 100644 --- a/godel_plugins/src/widgets/blend_tool_param_window.cpp +++ b/godel_plugins/src/widgets/blend_tool_param_window.cpp @@ -1,7 +1,8 @@ #include "godel_plugins/widgets/blend_tool_param_window.h" -godel_plugins::BlendingPlanConfigWidget::BlendingPlanConfigWidget(const godel_msgs::BlendingPlanParameters& params) - : params_(params) +godel_plugins::BlendingPlanConfigWidget::BlendingPlanConfigWidget( + const godel_msgs::BlendingPlanParameters& params) + : params_(params) { ui_.setupUi(this); @@ -29,5 +30,4 @@ void godel_plugins::BlendingPlanConfigWidget::update_internal_values() params_.tool_radius = ui_.LineEditSpindleDiameter->text().toDouble(); params_.overlap = ui_.lineEditOverlap->text().toDouble(); params_.margin = ui_.LineEditMargin->text().toDouble(); - } diff --git a/godel_plugins/src/widgets/parameter_window_base.cpp b/godel_plugins/src/widgets/parameter_window_base.cpp index 80aeed85..d72a9a4a 100644 --- a/godel_plugins/src/widgets/parameter_window_base.cpp +++ b/godel_plugins/src/widgets/parameter_window_base.cpp @@ -13,10 +13,7 @@ void godel_plugins::ParameterWindowBase::accept_changes_handler() hide(); } -void godel_plugins::ParameterWindowBase::cancel_changes_handler() -{ - hide(); -} +void godel_plugins::ParameterWindowBase::cancel_changes_handler() { hide(); } void godel_plugins::ParameterWindowBase::save_changes_handler() { diff --git a/godel_plugins/src/widgets/robot_blending_widget.cpp b/godel_plugins/src/widgets/robot_blending_widget.cpp index 2ae40a24..d6eac5ff 100644 --- a/godel_plugins/src/widgets/robot_blending_widget.cpp +++ b/godel_plugins/src/widgets/robot_blending_widget.cpp @@ -1,17 +1,17 @@ /* - Copyright Mar 13, 2014 Southwest Research Institute + Copyright Mar 13, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -24,607 +24,621 @@ #include #include -const double RAD_TO_DEGREES = 180.0f/M_PI; -const double DEGREES_TO_RAD = M_PI/180.0f; +const double RAD_TO_DEGREES = 180.0f / M_PI; +const double DEGREES_TO_RAD = M_PI / 180.0f; namespace godel_plugins { -namespace widgets { +namespace widgets +{ -RobotBlendingWidget::RobotBlendingWidget(std::string ns): - param_ns_(ns) +RobotBlendingWidget::RobotBlendingWidget(std::string ns) : param_ns_(ns) { - // TODO Auto-generated constructor stub - init(); + // TODO Auto-generated constructor stub + init(); } -RobotBlendingWidget::~RobotBlendingWidget() { - delete robot_scan_config_window_; - delete surface_detect_config_window_; - delete robot_blend_config_window_; - delete scan_plan_config_window_; +RobotBlendingWidget::~RobotBlendingWidget() +{ + delete robot_scan_config_window_; + delete surface_detect_config_window_; + delete robot_blend_config_window_; + delete scan_plan_config_window_; } void RobotBlendingWidget::init() { - // initializing ros comm interface - ros::NodeHandle nh(""); - surface_detection_client_ = nh.serviceClient(SURFACE_DETECTION_SERVICE); - surface_blending_parameters_client_ = nh.serviceClient(SURFACE_BLENDING_PARAMETERS_SERVICE); - select_surface_client_ = nh.serviceClient(SELECT_SURFACE_SERVICE); - process_plan_client_ = nh.serviceClient(PROCESS_PATH_SERVICE); - get_motion_plans_client_ = nh.serviceClient(GET_AVAILABLE_MOTION_PLANS_SERVICE); - select_motion_plan_client_ = nh.serviceClient(SELECT_MOTION_PLAN_SERVICE); - load_save_motion_plan_client_ = nh.serviceClient(LOAD_SAVE_MOTION_PLAN_SERVICE); - rename_surface_client_ = nh.serviceClient(RENAME_SURFACE_SERVICE); - - selected_surfaces_subs_ = nh.subscribe(SELECTED_SURFACES_CHANGED_TOPIC,1, - &RobotBlendingWidget::selected_surface_changed_callback,this); - - // initializing gui - ui_.setupUi(this); - - // initializing config windows - robot_scan_config_window_= new RobotScanConfigWidget(robot_scan_parameters_); - surface_detect_config_window_ = new SurfaceDetectionConfigWidget(surf_detect_parameters_); - - robot_blend_config_window_= new BlendingPlanConfigWidget(blending_plan_parameters_); - scan_plan_config_window_=new ScanPlanConfigWidget(scan_plan_parameters_); - - // setting signals and slots - connect(robot_scan_config_window_,SIGNAL(parameters_changed()),this,SLOT(robot_scan_params_changed_handler())); - connect(surface_detect_config_window_,SIGNAL(parameters_changed()),this,SLOT(surface_detect_params_changed_handler())); - connect(ui_.PushButtonMoreOptions,SIGNAL(clicked()),this,SLOT(scan_options_click_handler())); - - connect(ui_.PushButtonBlendOptions,SIGNAL(clicked()),this,SLOT(blend_options_click_handler())); - connect(ui_.pushButtonProfileOptions,SIGNAL(clicked()),this,SLOT(scan_plan_options_click_handler())); - - connect(ui_.PushButtonSurfaceOptions,SIGNAL(clicked()),this,SLOT(surface_options_click_handler())); - connect(ui_.PushButtonScan,SIGNAL(clicked()),this,SLOT(scan_button_handler())); - connect(ui_.PushButtonFindSurface,SIGNAL(clicked()),this,SLOT(find_surface_button_handler())); - connect(ui_.PushButtonNext,SIGNAL(clicked()),this,SLOT(increase_tab_index_handler())); - connect(ui_.PushButtonBack,SIGNAL(clicked()),this,SLOT(decrease_tab_index_handler())); - connect(ui_.PushButtonSelectAllSurfaces,SIGNAL(clicked()),this,SLOT(select_all_handler())); - connect(ui_.PushButtonDeselectAllSurfaces,SIGNAL(clicked()),this,SLOT(deselect_all_handler())); - connect(ui_.PushButtonHideAllSurfaces,SIGNAL(clicked()),this,SLOT(hide_all_handler())); - connect(ui_.PushButtonShowAllSurfaces,SIGNAL(clicked()),this,SLOT(show_all_handler())); - connect(ui_.PushButtonPreviewPath,SIGNAL(clicked()),this,SLOT(preview_path_handler())); - connect(ui_.PushButtonGeneratePaths,SIGNAL(clicked()),this,SLOT(generate_process_path_handler())); - connect(this,SIGNAL(selection_changed()),this,SLOT(selection_changed_handler())); - connect(this,SIGNAL(surface_detection_started()),this,SLOT(surface_detection_started_handler())); - connect(this,SIGNAL(surface_detection_completed()),this,SLOT(surface_detection_completed_handler())); - connect(this,SIGNAL(connect_started()),this,SLOT(connect_started_handler())); - connect(this,SIGNAL(connect_completed()),this,SLOT(connect_completed_handler())); - - connect(ui_.pushButtonSavePlan, SIGNAL(clicked(bool)), this, SLOT(save_motion_plan_handler())); - connect(ui_.PushButtonOpenFile, SIGNAL(clicked(bool)), this, SLOT(load_motion_plan_handler())); - - connect(ui_.pushButtonExecutePath, SIGNAL(clicked()), this, SLOT(execute_motion_plan_handler())); - connect(ui_.pushButtonSimulatePath, SIGNAL(clicked(bool)), this, SLOT(simulate_motion_plan_handler())); - - connect(robot_scan_config_window_, SIGNAL(parameters_save_requested()), this, SLOT(request_save_parameters())); - connect(surface_detect_config_window_, SIGNAL(parameters_save_requested()), this, SLOT(request_save_parameters())); - connect(robot_blend_config_window_, SIGNAL(parameters_save_requested()), this, SLOT(request_save_parameters())); - connect(ui_.ListWidgetSelectedSurfs, SIGNAL(itemDoubleClicked(QListWidgetItem*)), - this, SLOT(handle_surface_rename(QListWidgetItem*))); - connect(scan_plan_config_window_, SIGNAL(parameters_save_requested()), this, SLOT(request_save_parameters())); - - // moving to first tab - ui_.TabWidgetCreateLib->setCurrentIndex(0); - - // setting up timer - QTimer *timer = new QTimer(this); - connect(timer,SIGNAL(timeout()),this,SLOT(update_handler())); - timer->start(4000); - - // connect from a separate thread - QFuture future = QtConcurrent::run(this,&RobotBlendingWidget::connect_to_services); + // initializing ros comm interface + ros::NodeHandle nh(""); + surface_detection_client_ = + nh.serviceClient(SURFACE_DETECTION_SERVICE); + surface_blending_parameters_client_ = + nh.serviceClient(SURFACE_BLENDING_PARAMETERS_SERVICE); + select_surface_client_ = nh.serviceClient(SELECT_SURFACE_SERVICE); + process_plan_client_ = nh.serviceClient(PROCESS_PATH_SERVICE); + get_motion_plans_client_ = + nh.serviceClient(GET_AVAILABLE_MOTION_PLANS_SERVICE); + select_motion_plan_client_ = + nh.serviceClient(SELECT_MOTION_PLAN_SERVICE); + load_save_motion_plan_client_ = + nh.serviceClient(LOAD_SAVE_MOTION_PLAN_SERVICE); + rename_surface_client_ = nh.serviceClient(RENAME_SURFACE_SERVICE); + + selected_surfaces_subs_ = + nh.subscribe(SELECTED_SURFACES_CHANGED_TOPIC, 1, + &RobotBlendingWidget::selected_surface_changed_callback, this); + + // initializing gui + ui_.setupUi(this); + + // initializing config windows + robot_scan_config_window_ = new RobotScanConfigWidget(robot_scan_parameters_); + surface_detect_config_window_ = new SurfaceDetectionConfigWidget(surf_detect_parameters_); + + robot_blend_config_window_ = new BlendingPlanConfigWidget(blending_plan_parameters_); + scan_plan_config_window_ = new ScanPlanConfigWidget(scan_plan_parameters_); + + // setting signals and slots + connect(robot_scan_config_window_, SIGNAL(parameters_changed()), this, + SLOT(robot_scan_params_changed_handler())); + connect(surface_detect_config_window_, SIGNAL(parameters_changed()), this, + SLOT(surface_detect_params_changed_handler())); + connect(ui_.PushButtonMoreOptions, SIGNAL(clicked()), this, SLOT(scan_options_click_handler())); + + connect(ui_.PushButtonBlendOptions, SIGNAL(clicked()), this, SLOT(blend_options_click_handler())); + connect(ui_.pushButtonProfileOptions, SIGNAL(clicked()), this, + SLOT(scan_plan_options_click_handler())); + + connect(ui_.PushButtonSurfaceOptions, SIGNAL(clicked()), this, + SLOT(surface_options_click_handler())); + connect(ui_.PushButtonScan, SIGNAL(clicked()), this, SLOT(scan_button_handler())); + connect(ui_.PushButtonFindSurface, SIGNAL(clicked()), this, SLOT(find_surface_button_handler())); + connect(ui_.PushButtonNext, SIGNAL(clicked()), this, SLOT(increase_tab_index_handler())); + connect(ui_.PushButtonBack, SIGNAL(clicked()), this, SLOT(decrease_tab_index_handler())); + connect(ui_.PushButtonSelectAllSurfaces, SIGNAL(clicked()), this, SLOT(select_all_handler())); + connect(ui_.PushButtonDeselectAllSurfaces, SIGNAL(clicked()), this, SLOT(deselect_all_handler())); + connect(ui_.PushButtonHideAllSurfaces, SIGNAL(clicked()), this, SLOT(hide_all_handler())); + connect(ui_.PushButtonShowAllSurfaces, SIGNAL(clicked()), this, SLOT(show_all_handler())); + connect(ui_.PushButtonPreviewPath, SIGNAL(clicked()), this, SLOT(preview_path_handler())); + connect(ui_.PushButtonGeneratePaths, SIGNAL(clicked()), this, + SLOT(generate_process_path_handler())); + connect(this, SIGNAL(selection_changed()), this, SLOT(selection_changed_handler())); + connect(this, SIGNAL(surface_detection_started()), this, + SLOT(surface_detection_started_handler())); + connect(this, SIGNAL(surface_detection_completed()), this, + SLOT(surface_detection_completed_handler())); + connect(this, SIGNAL(connect_started()), this, SLOT(connect_started_handler())); + connect(this, SIGNAL(connect_completed()), this, SLOT(connect_completed_handler())); + + connect(ui_.pushButtonSavePlan, SIGNAL(clicked(bool)), this, SLOT(save_motion_plan_handler())); + connect(ui_.PushButtonOpenFile, SIGNAL(clicked(bool)), this, SLOT(load_motion_plan_handler())); + + connect(ui_.pushButtonExecutePath, SIGNAL(clicked()), this, SLOT(execute_motion_plan_handler())); + connect(ui_.pushButtonSimulatePath, SIGNAL(clicked(bool)), this, + SLOT(simulate_motion_plan_handler())); + + connect(robot_scan_config_window_, SIGNAL(parameters_save_requested()), this, + SLOT(request_save_parameters())); + connect(surface_detect_config_window_, SIGNAL(parameters_save_requested()), this, + SLOT(request_save_parameters())); + connect(robot_blend_config_window_, SIGNAL(parameters_save_requested()), this, + SLOT(request_save_parameters())); + connect(ui_.ListWidgetSelectedSurfs, SIGNAL(itemDoubleClicked(QListWidgetItem*)), this, + SLOT(handle_surface_rename(QListWidgetItem*))); + connect(scan_plan_config_window_, SIGNAL(parameters_save_requested()), this, + SLOT(request_save_parameters())); + + // moving to first tab + ui_.TabWidgetCreateLib->setCurrentIndex(0); + + // setting up timer + QTimer* timer = new QTimer(this); + connect(timer, SIGNAL(timeout()), this, SLOT(update_handler())); + timer->start(4000); + + // connect from a separate thread + QFuture future = QtConcurrent::run(this, &RobotBlendingWidget::connect_to_services); } void RobotBlendingWidget::connect_to_services() { - // call services to get parameters - godel_msgs::SurfaceBlendingParameters::Request req; - godel_msgs::SurfaceBlendingParameters::Response res; - req.action = req.GET_CURRENT_PARAMETERS; - - // disable gui - Q_EMIT connect_started(); - - // wait for services to connect - while(ros::ok()) - { - ROS_INFO_STREAM("rviz blending panel connecting to services"); - if(surface_detection_client_.waitForExistence(ros::Duration(2)) && - select_surface_client_.waitForExistence(ros::Duration(2)) && - surface_blending_parameters_client_.waitForExistence(ros::Duration(2))) - { - - ROS_INFO_STREAM("rviz panel connected to the services '"<params() = res.robot_scan; - surface_detect_config_window_->params() = res.surface_detection; - robot_scan_parameters_ = res.robot_scan; - surf_detect_parameters_ = res.surface_detection; - blending_plan_parameters_ = res.blending_plan; - robot_blend_config_window_->params() = res.blending_plan; - scan_plan_config_window_->params() = res.scan_plan; - - // update gui elements for robot scan - robot_scan_params_changed_handler(); - - // enable gui - Q_EMIT connect_completed(); - - ROS_INFO_STREAM("Call to service for parameters succeeded"); - break; - } - else - { - ROS_ERROR_STREAM("Call to service for parameters failed"); - } - } - else - { - ROS_ERROR_STREAM("rviz panel could not connect to one or more ros services:\n\t'"<params() = res.robot_scan; + surface_detect_config_window_->params() = res.surface_detection; + robot_scan_parameters_ = res.robot_scan; + surf_detect_parameters_ = res.surface_detection; + blending_plan_parameters_ = res.blending_plan; + robot_blend_config_window_->params() = res.blending_plan; + scan_plan_config_window_->params() = res.scan_plan; + + // update gui elements for robot scan + robot_scan_params_changed_handler(); + + // enable gui + Q_EMIT connect_completed(); + + ROS_INFO_STREAM("Call to service for parameters succeeded"); + break; + } + else + { + ROS_ERROR_STREAM("Call to service for parameters failed"); + } + } + else + { + ROS_ERROR_STREAM("rviz panel could not connect to one or more ros services:\n\t'" + << surface_detection_client_.getService() << "'\n\t'" + << select_surface_client_.getService() << "'\n\t'" + << surface_blending_parameters_client_.getService()); + } + } +} + +bool RobotBlendingWidget::call_select_surface_service(godel_msgs::SelectSurface::Request& req) +{ + godel_msgs::SelectSurface::Response res; + bool succeeded = select_surface_client_.call(req, res); + if (succeeded) + { + } + else + { + } + + return succeeded; } bool RobotBlendingWidget::call_surface_detection_service(godel_msgs::SurfaceDetection& s) { - bool succeeded = surface_detection_client_.call(s.request,s.response); - if(succeeded) - { - - } - else - { + bool succeeded = surface_detection_client_.call(s.request, s.response); + if (succeeded) + { + } + else + { + } - } - - return succeeded; + return succeeded; } -void RobotBlendingWidget::selected_surface_changed_callback(godel_msgs::SelectedSurfacesChangedConstPtr msg) +void RobotBlendingWidget::selected_surface_changed_callback( + godel_msgs::SelectedSurfacesChangedConstPtr msg) { - selected_surfaces_msg_ = *msg; - emit_signal_selection_change(); + selected_surfaces_msg_ = *msg; + emit_signal_selection_change(); } void RobotBlendingWidget::select_all_handler() { - godel_msgs::SelectSurface::Request req; - req.action = req.SELECT_ALL; - call_select_surface_service(req); + godel_msgs::SelectSurface::Request req; + req.action = req.SELECT_ALL; + call_select_surface_service(req); } void RobotBlendingWidget::deselect_all_handler() { - godel_msgs::SelectSurface::Request req; - req.action = req.DESELECT_ALL; - call_select_surface_service(req); + godel_msgs::SelectSurface::Request req; + req.action = req.DESELECT_ALL; + call_select_surface_service(req); } void RobotBlendingWidget::hide_all_handler() { - godel_msgs::SelectSurface::Request req; - req.action = req.HIDE_ALL; - call_select_surface_service(req); + godel_msgs::SelectSurface::Request req; + req.action = req.HIDE_ALL; + call_select_surface_service(req); } void RobotBlendingWidget::show_all_handler() { - godel_msgs::SelectSurface::Request req; - req.action = req.SHOW_ALL; - call_select_surface_service(req); + godel_msgs::SelectSurface::Request req; + req.action = req.SHOW_ALL; + call_select_surface_service(req); } void RobotBlendingWidget::robot_scan_params_changed_handler() { - robot_scan_parameters_ = robot_scan_config_window_->params(); + robot_scan_parameters_ = robot_scan_config_window_->params(); - // updating entries in gui - ui_.LineEditSensorTopic->setText(QString::fromStdString(robot_scan_parameters_.scan_topic)); - ui_.SpinBoxNumScans->setValue(static_cast(robot_scan_parameters_.num_scan_points)); - ui_.LineEditCamTilt->setText(QString::number(RAD_TO_DEGREES* robot_scan_parameters_.cam_tilt_angle)); - ui_.LineEditSweepAngleStart->setText(QString::number(RAD_TO_DEGREES* robot_scan_parameters_.sweep_angle_start)); - ui_.LineEditSweepAngleEnd->setText(QString::number(RAD_TO_DEGREES* robot_scan_parameters_.sweep_angle_end)); + // updating entries in gui + ui_.LineEditSensorTopic->setText(QString::fromStdString(robot_scan_parameters_.scan_topic)); + ui_.SpinBoxNumScans->setValue(static_cast(robot_scan_parameters_.num_scan_points)); + ui_.LineEditCamTilt->setText( + QString::number(RAD_TO_DEGREES * robot_scan_parameters_.cam_tilt_angle)); + ui_.LineEditSweepAngleStart->setText( + QString::number(RAD_TO_DEGREES * robot_scan_parameters_.sweep_angle_start)); + ui_.LineEditSweepAngleEnd->setText( + QString::number(RAD_TO_DEGREES * robot_scan_parameters_.sweep_angle_end)); - // request publish scan path - godel_msgs::SurfaceDetection s; - s.request.action = s.request.PUBLISH_SCAN_PATH; - s.request.use_default_parameters = false; - s.request.robot_scan = robot_scan_parameters_; - s.request.surface_detection = surf_detect_parameters_; - call_surface_detection_service(s); + // request publish scan path + godel_msgs::SurfaceDetection s; + s.request.action = s.request.PUBLISH_SCAN_PATH; + s.request.use_default_parameters = false; + s.request.robot_scan = robot_scan_parameters_; + s.request.surface_detection = surf_detect_parameters_; + call_surface_detection_service(s); } void RobotBlendingWidget::surface_detect_params_changed_handler() { - surf_detect_parameters_ = surface_detect_config_window_->params(); + surf_detect_parameters_ = surface_detect_config_window_->params(); } void RobotBlendingWidget::preview_path_handler() { - save_robot_scan_parameters(); + save_robot_scan_parameters(); - // request publish scan path - godel_msgs::SurfaceDetection s; - s.request.action = s.request.PUBLISH_SCAN_PATH; - s.request.use_default_parameters = false; - s.request.robot_scan = robot_scan_parameters_; - s.request.surface_detection = surf_detect_parameters_; - call_surface_detection_service(s); + // request publish scan path + godel_msgs::SurfaceDetection s; + s.request.action = s.request.PUBLISH_SCAN_PATH; + s.request.use_default_parameters = false; + s.request.robot_scan = robot_scan_parameters_; + s.request.surface_detection = surf_detect_parameters_; + call_surface_detection_service(s); } void RobotBlendingWidget::scan_options_click_handler() { - save_robot_scan_parameters(); - robot_scan_config_window_->params() = robot_scan_parameters_; - robot_scan_config_window_->show(); + save_robot_scan_parameters(); + robot_scan_config_window_->params() = robot_scan_parameters_; + robot_scan_config_window_->show(); } -void RobotBlendingWidget::blend_options_click_handler() -{ - robot_blend_config_window_->show(); -} +void RobotBlendingWidget::blend_options_click_handler() { robot_blend_config_window_->show(); } -void RobotBlendingWidget::scan_plan_options_click_handler() -{ - scan_plan_config_window_->show(); -} +void RobotBlendingWidget::scan_plan_options_click_handler() { scan_plan_config_window_->show(); } void RobotBlendingWidget::surface_options_click_handler() { - surface_detect_config_window_->params() = surf_detect_parameters_; - surface_detect_config_window_->show(); + surface_detect_config_window_->params() = surf_detect_parameters_; + surface_detect_config_window_->show(); } - -void RobotBlendingWidget::update_handler() -{ - -} +void RobotBlendingWidget::update_handler() {} void RobotBlendingWidget::selection_changed_handler() { - std::vector list = selected_surfaces_msg_.selected_surfaces; + std::vector list = selected_surfaces_msg_.selected_surfaces; - ui_.ListWidgetSelectedSurfs->clear(); - if(list.size() > 0) - { - for(std::vector::iterator i = list.begin(); i != list.end();i++) - { - QListWidgetItem *item = new QListWidgetItem(); - item->setText(QString::fromStdString(*i)); - ui_.ListWidgetSelectedSurfs->addItem(item); - } - } + ui_.ListWidgetSelectedSurfs->clear(); + if (list.size() > 0) + { + for (std::vector::iterator i = list.begin(); i != list.end(); i++) + { + QListWidgetItem* item = new QListWidgetItem(); + item->setText(QString::fromStdString(*i)); + ui_.ListWidgetSelectedSurfs->addItem(item); + } + } } void RobotBlendingWidget::increase_tab_index_handler() { - ui_.TabWidgetCreateLib->setCurrentIndex(ui_.TabWidgetCreateLib->currentIndex() + 1); + ui_.TabWidgetCreateLib->setCurrentIndex(ui_.TabWidgetCreateLib->currentIndex() + 1); } void RobotBlendingWidget::decrease_tab_index_handler() { - ui_.TabWidgetCreateLib->setCurrentIndex(ui_.TabWidgetCreateLib->currentIndex() - 1); + ui_.TabWidgetCreateLib->setCurrentIndex(ui_.TabWidgetCreateLib->currentIndex() - 1); } void RobotBlendingWidget::scan_button_handler() { - QFuture future = QtConcurrent::run(this,&RobotBlendingWidget::send_scan_and_find_request); + QFuture future = QtConcurrent::run(this, &RobotBlendingWidget::send_scan_and_find_request); } void RobotBlendingWidget::find_surface_button_handler() { - QFuture future = QtConcurrent::run(this,&RobotBlendingWidget::send_find_surface_request); + QFuture future = QtConcurrent::run(this, &RobotBlendingWidget::send_find_surface_request); } void RobotBlendingWidget::send_find_surface_request() { - surface_detection_op_message_ = "FIND IN PROGRESS"; - - // disable gui - Q_EMIT surface_detection_started(); - - // creating surface detection request - godel_msgs::SurfaceDetection s; - s.request.action = s.request.FIND_ONLY; - s.request.use_default_parameters = false; - s.request.robot_scan = robot_scan_parameters_; - s.request.surface_detection = surf_detect_parameters_; - - if(call_surface_detection_service(s) ) - { - if(s.response.surfaces_found) - { - surface_detection_op_succeeded_ = true; - surface_detection_op_message_ = "FIND COMPLETED"; - } - else - { - surface_detection_op_succeeded_ = false; - surface_detection_op_message_ = "FIND FAILED"; - } - } - else - { - surface_detection_op_succeeded_ = false; - surface_detection_op_message_ = "SERVICE CALL FAILED"; - } - - // enable widget - Q_EMIT surface_detection_completed(); + surface_detection_op_message_ = "FIND IN PROGRESS"; + + // disable gui + Q_EMIT surface_detection_started(); + + // creating surface detection request + godel_msgs::SurfaceDetection s; + s.request.action = s.request.FIND_ONLY; + s.request.use_default_parameters = false; + s.request.robot_scan = robot_scan_parameters_; + s.request.surface_detection = surf_detect_parameters_; + + if (call_surface_detection_service(s)) + { + if (s.response.surfaces_found) + { + surface_detection_op_succeeded_ = true; + surface_detection_op_message_ = "FIND COMPLETED"; + } + else + { + surface_detection_op_succeeded_ = false; + surface_detection_op_message_ = "FIND FAILED"; + } + } + else + { + surface_detection_op_succeeded_ = false; + surface_detection_op_message_ = "SERVICE CALL FAILED"; + } + + // enable widget + Q_EMIT surface_detection_completed(); } - void RobotBlendingWidget::send_scan_and_find_request() { - surface_detection_op_message_ = "SCAN & FIND IN PROGRESS"; - - // disable gui - Q_EMIT surface_detection_started(); - - - // saving parameters - save_robot_scan_parameters(); - - // creating surface detection request - godel_msgs::SurfaceDetection s; - s.request.action = s.request.SCAN_AND_FIND_ONLY; - s.request.use_default_parameters = false; - s.request.robot_scan = robot_scan_parameters_; - s.request.surface_detection = surf_detect_parameters_; - - if(call_surface_detection_service(s) ) - { - if(s.response.surfaces_found) - { - surface_detection_op_succeeded_ = true; - surface_detection_op_message_ = "SCAN & FIND COMPLETED"; - } - else - { - surface_detection_op_succeeded_ = false; - surface_detection_op_message_ = "SCAN & FIND FAILED"; - } - } - else - { - surface_detection_op_succeeded_ = false; - surface_detection_op_message_ = "SERVICE CALL FAILED"; - } - - Q_EMIT surface_detection_completed(); + surface_detection_op_message_ = "SCAN & FIND IN PROGRESS"; + + // disable gui + Q_EMIT surface_detection_started(); + + // saving parameters + save_robot_scan_parameters(); + + // creating surface detection request + godel_msgs::SurfaceDetection s; + s.request.action = s.request.SCAN_AND_FIND_ONLY; + s.request.use_default_parameters = false; + s.request.robot_scan = robot_scan_parameters_; + s.request.surface_detection = surf_detect_parameters_; + + if (call_surface_detection_service(s)) + { + if (s.response.surfaces_found) + { + surface_detection_op_succeeded_ = true; + surface_detection_op_message_ = "SCAN & FIND COMPLETED"; + } + else + { + surface_detection_op_succeeded_ = false; + surface_detection_op_message_ = "SCAN & FIND FAILED"; + } + } + else + { + surface_detection_op_succeeded_ = false; + surface_detection_op_message_ = "SERVICE CALL FAILED"; + } + + Q_EMIT surface_detection_completed(); } void RobotBlendingWidget::surface_detection_started_handler() { - ui_.LineEditOperationStatus->setText(QString::fromStdString(surface_detection_op_message_)); - ui_.TabWidget->setEnabled(false); + ui_.LineEditOperationStatus->setText(QString::fromStdString(surface_detection_op_message_)); + ui_.TabWidget->setEnabled(false); } void RobotBlendingWidget::surface_detection_completed_handler() { - ui_.LineEditOperationStatus->setText(QString::fromStdString(surface_detection_op_message_)); - if(surface_detection_op_succeeded_) - { - ui_.TabWidgetCreateLib->setCurrentIndex(1); - } - ui_.TabWidget->setEnabled(true); + ui_.LineEditOperationStatus->setText(QString::fromStdString(surface_detection_op_message_)); + if (surface_detection_op_succeeded_) + { + ui_.TabWidgetCreateLib->setCurrentIndex(1); + } + ui_.TabWidget->setEnabled(true); } void RobotBlendingWidget::connect_started_handler() { - ui_.LineEditOperationStatus->setText("CONNECTING TO SERVICE"); - ui_.TabWidget->setEnabled(false); + ui_.LineEditOperationStatus->setText("CONNECTING TO SERVICE"); + ui_.TabWidget->setEnabled(false); } void RobotBlendingWidget::connect_completed_handler() { - ui_.LineEditOperationStatus->setText("READY"); - ui_.TabWidget->setEnabled(true); + ui_.LineEditOperationStatus->setText("READY"); + ui_.TabWidget->setEnabled(true); } void RobotBlendingWidget::generate_process_path_handler() { - godel_msgs::ProcessPlanning process_plan; - process_plan.request.use_default_parameters = false; - process_plan.request.params = robot_blend_config_window_->params(); - process_plan.request.scan_params = this->scan_plan_config_window_->params(); - process_plan.request.action = process_plan.request.GENERATE_MOTION_PLAN_AND_PREVIEW; - ROS_INFO_STREAM("process plan request sent"); - if (process_plan_client_.call(process_plan)) - { - std::vector plan_names; - request_available_motions(plan_names); - update_motion_plan_list(plan_names); + godel_msgs::ProcessPlanning process_plan; + process_plan.request.use_default_parameters = false; + process_plan.request.params = robot_blend_config_window_->params(); + process_plan.request.scan_params = this->scan_plan_config_window_->params(); + process_plan.request.action = process_plan.request.GENERATE_MOTION_PLAN_AND_PREVIEW; + ROS_INFO_STREAM("process plan request sent"); + if (process_plan_client_.call(process_plan)) + { + std::vector plan_names; + request_available_motions(plan_names); + update_motion_plan_list(plan_names); - ui_.TabWidgetCreateLib->setCurrentIndex(2); - } + ui_.TabWidgetCreateLib->setCurrentIndex(2); + } } void RobotBlendingWidget::update_motion_plan_list(const std::vector& names) { - ui_.ListPathResults->clear(); - for (std::size_t i = 0; i < names.size(); ++i) - { - QListWidgetItem *item = new QListWidgetItem(); - item->setText(QString::fromStdString(names[i])); - ui_.ListPathResults->addItem(item); - } + ui_.ListPathResults->clear(); + for (std::size_t i = 0; i < names.size(); ++i) + { + QListWidgetItem* item = new QListWidgetItem(); + item->setText(QString::fromStdString(names[i])); + ui_.ListPathResults->addItem(item); + } } void RobotBlendingWidget::save_robot_scan_parameters() { - robot_scan_parameters_.num_scan_points = ui_.SpinBoxNumScans->value(); - robot_scan_parameters_.cam_tilt_angle = ui_.LineEditCamTilt->text().toDouble()*DEGREES_TO_RAD; - robot_scan_parameters_.sweep_angle_start = ui_.LineEditSweepAngleStart->text().toDouble()*DEGREES_TO_RAD; - robot_scan_parameters_.sweep_angle_end = ui_.LineEditSweepAngleEnd->text().toDouble()*DEGREES_TO_RAD; - robot_scan_parameters_.scan_topic = ui_.LineEditSensorTopic->text().toStdString(); + robot_scan_parameters_.num_scan_points = ui_.SpinBoxNumScans->value(); + robot_scan_parameters_.cam_tilt_angle = ui_.LineEditCamTilt->text().toDouble() * DEGREES_TO_RAD; + robot_scan_parameters_.sweep_angle_start = + ui_.LineEditSweepAngleStart->text().toDouble() * DEGREES_TO_RAD; + robot_scan_parameters_.sweep_angle_end = + ui_.LineEditSweepAngleEnd->text().toDouble() * DEGREES_TO_RAD; + robot_scan_parameters_.scan_topic = ui_.LineEditSensorTopic->text().toStdString(); } -void RobotBlendingWidget::request_available_motions(std::vector &plans) +void RobotBlendingWidget::request_available_motions(std::vector& plans) { - godel_msgs::GetAvailableMotionPlans srv; - if (get_motion_plans_client_.call(srv)) - { - plans = srv.response.names; - } - else - { - ROS_ERROR_STREAM("Could not get names from 'available motions server'"); - } + godel_msgs::GetAvailableMotionPlans srv; + if (get_motion_plans_client_.call(srv)) + { + plans = srv.response.names; + } + else + { + ROS_ERROR_STREAM("Could not get names from 'available motions server'"); + } } -void RobotBlendingWidget::select_motion_plan(const std::string &name, bool simulate) +void RobotBlendingWidget::select_motion_plan(const std::string& name, bool simulate) { - godel_msgs::SelectMotionPlan srv; - srv.request.name = name; - srv.request.simulate = simulate; - select_motion_plan_client_.call(srv); + godel_msgs::SelectMotionPlan srv; + srv.request.name = name; + srv.request.simulate = simulate; + select_motion_plan_client_.call(srv); } - void RobotBlendingWidget::simulate_motion_plan_handler() { - if (ui_.ListPathResults->currentItem() == NULL) return; - std::string name = ui_.ListPathResults->currentItem()->text().toStdString(); - ROS_INFO_STREAM("Selected " << name << " to be simulated"); - if (!name.empty()) select_motion_plan(name, true); + if (ui_.ListPathResults->currentItem() == NULL) + return; + std::string name = ui_.ListPathResults->currentItem()->text().toStdString(); + ROS_INFO_STREAM("Selected " << name << " to be simulated"); + if (!name.empty()) + select_motion_plan(name, true); } void RobotBlendingWidget::execute_motion_plan_handler() { - if (ui_.ListPathResults->currentItem() == NULL) return; - std::string name = ui_.ListPathResults->currentItem()->text().toStdString(); - ROS_INFO_STREAM("Selected " << name << " to be executed"); - if (!name.empty()) select_motion_plan(name, false); + if (ui_.ListPathResults->currentItem() == NULL) + return; + std::string name = ui_.ListPathResults->currentItem()->text().toStdString(); + ROS_INFO_STREAM("Selected " << name << " to be executed"); + if (!name.empty()) + select_motion_plan(name, false); } void RobotBlendingWidget::save_motion_plan_handler() { - QString filepath = QFileDialog::getSaveFileName(this, "Save Motion Plan"); - std::string path = filepath.toStdString(); - if (!path.empty()) - { - ROS_DEBUG_STREAM("You want to save motion plan to: " << path); - request_load_save_motions(path, false); - } + QString filepath = QFileDialog::getSaveFileName(this, "Save Motion Plan"); + std::string path = filepath.toStdString(); + if (!path.empty()) + { + ROS_DEBUG_STREAM("You want to save motion plan to: " << path); + request_load_save_motions(path, false); + } } void RobotBlendingWidget::load_motion_plan_handler() { - QString filepath = QFileDialog::getOpenFileName(this, "Load Motion Plan"); - std::string path = filepath.toStdString(); - if (!path.empty()) - { - ROS_DEBUG_STREAM("You want to load a motion plan from: " << path); - request_load_save_motions(path, true); - } + QString filepath = QFileDialog::getOpenFileName(this, "Load Motion Plan"); + std::string path = filepath.toStdString(); + if (!path.empty()) + { + ROS_DEBUG_STREAM("You want to load a motion plan from: " << path); + request_load_save_motions(path, true); + } } void RobotBlendingWidget::request_load_save_motions(const std::string& path, bool isLoad) { - // Pre-condition: The path must not be an empty string - if (path.empty()) - { - ROS_WARN("Cannot save or load an empty path"); - return; - } - - godel_msgs::LoadSaveMotionPlan srv; - srv.request.path = path; - - if (isLoad) - { - srv.request.mode = godel_msgs::LoadSaveMotionPlan::Request::MODE_LOAD; - } - else - { - srv.request.mode = godel_msgs::LoadSaveMotionPlan::Request::MODE_SAVE; - } - - if (load_save_motion_plan_client_.call(srv)) - { - std::vector plans; - request_available_motions(plans); - update_motion_plan_list(plans); - } - else - { - ROS_WARN_STREAM("Blending service unable to " << (isLoad ? "load" : "save") << "plan: " << path); - } + // Pre-condition: The path must not be an empty string + if (path.empty()) + { + ROS_WARN("Cannot save or load an empty path"); + return; + } + + godel_msgs::LoadSaveMotionPlan srv; + srv.request.path = path; + + if (isLoad) + { + srv.request.mode = godel_msgs::LoadSaveMotionPlan::Request::MODE_LOAD; + } + else + { + srv.request.mode = godel_msgs::LoadSaveMotionPlan::Request::MODE_SAVE; + } + + if (load_save_motion_plan_client_.call(srv)) + { + std::vector plans; + request_available_motions(plans); + update_motion_plan_list(plans); + } + else + { + ROS_WARN_STREAM("Blending service unable to " << (isLoad ? "load" : "save") + << "plan: " << path); + } } void RobotBlendingWidget::request_save_parameters() { - godel_msgs::SurfaceBlendingParameters::Request req; - req.action = godel_msgs::SurfaceBlendingParameters::Request::SAVE_PARAMETERS; - req.surface_detection = surface_detect_config_window_->params(); - req.blending_plan = robot_blend_config_window_->params(); - req.robot_scan = robot_scan_config_window_->params(); - req.scan_plan = scan_plan_config_window_->params(); - - godel_msgs::SurfaceBlendingParameters::Response res; - if (!surface_blending_parameters_client_.call(req, res)) - { - ROS_WARN_STREAM("Could not complete service call to save your parameters!"); - } -} - -void RobotBlendingWidget::handle_surface_rename(QListWidgetItem *item) -{ - if (!item) return; - - QString old_text = item->text(); - // spawn window to prompt a rename - QString new_text = QInputDialog::getText(this, "Surface Rename", "Enter a new surface name: "); - - if (!new_text.isEmpty()) - { - godel_msgs::RenameSurfaceResponse res; - godel_msgs::RenameSurfaceRequest req; - req.old_name = old_text.toStdString(); - req.new_name = new_text.toStdString(); - if (rename_surface_client_.call(req, res)) - { - item->setText(new_text); - } - else - { - ROS_WARN_STREAM("Failed to update the name of surface " << old_text.toStdString()); - } - } + godel_msgs::SurfaceBlendingParameters::Request req; + req.action = godel_msgs::SurfaceBlendingParameters::Request::SAVE_PARAMETERS; + req.surface_detection = surface_detect_config_window_->params(); + req.blending_plan = robot_blend_config_window_->params(); + req.robot_scan = robot_scan_config_window_->params(); + req.scan_plan = scan_plan_config_window_->params(); + + godel_msgs::SurfaceBlendingParameters::Response res; + if (!surface_blending_parameters_client_.call(req, res)) + { + ROS_WARN_STREAM("Could not complete service call to save your parameters!"); + } +} + +void RobotBlendingWidget::handle_surface_rename(QListWidgetItem* item) +{ + if (!item) + return; + + QString old_text = item->text(); + // spawn window to prompt a rename + QString new_text = QInputDialog::getText(this, "Surface Rename", "Enter a new surface name: "); + + if (!new_text.isEmpty()) + { + godel_msgs::RenameSurfaceResponse res; + godel_msgs::RenameSurfaceRequest req; + req.old_name = old_text.toStdString(); + req.new_name = new_text.toStdString(); + if (rename_surface_client_.call(req, res)) + { + item->setText(new_text); + } + else + { + ROS_WARN_STREAM("Failed to update the name of surface " << old_text.toStdString()); + } + } } } /* namespace widgets */ - } diff --git a/godel_plugins/src/widgets/robot_scan_configuration.cpp b/godel_plugins/src/widgets/robot_scan_configuration.cpp index 43d42d69..f85c1087 100644 --- a/godel_plugins/src/widgets/robot_scan_configuration.cpp +++ b/godel_plugins/src/widgets/robot_scan_configuration.cpp @@ -9,8 +9,7 @@ #endif // Pose Widget -godel_plugins::PoseWidget::PoseWidget(QWidget *parent) - : QWidget(parent) +godel_plugins::PoseWidget::PoseWidget(QWidget* parent) : QWidget(parent) { ui_.setupUi(this); set_values(tf::Transform::getIdentity()); @@ -19,15 +18,15 @@ godel_plugins::PoseWidget::PoseWidget(QWidget *parent) void godel_plugins::PoseWidget::set_values(const geometry_msgs::Pose& p) { tf::Transform t; - tf::poseMsgToTF(p,t); + tf::poseMsgToTF(p, t); set_values(t); } void godel_plugins::PoseWidget::set_values(const tf::Transform& t) { tf::Vector3 p = t.getOrigin(); - tfScalar rx,ry,rz; - t.getBasis().getRPY(rx,ry,rz,1); + tfScalar rx, ry, rz; + t.getBasis().getRPY(rx, ry, rz, 1); ui_.LineEditX->setText(QString::number(p.x())); ui_.LineEditY->setText(QString::number(p.y())); ui_.LineEditZ->setText(QString::number(p.z())); @@ -38,7 +37,7 @@ void godel_plugins::PoseWidget::set_values(const tf::Transform& t) tf::Transform godel_plugins::PoseWidget::get_values() { - double x,y,z,rx,ry,rz; + double x, y, z, rx, ry, rz; x = ui_.LineEditX->text().toDouble(); y = ui_.LineEditY->text().toDouble(); z = ui_.LineEditZ->text().toDouble(); @@ -47,16 +46,17 @@ tf::Transform godel_plugins::PoseWidget::get_values() rz = DEG2RAD(ui_.LineEditRz->text().toDouble()); // create transform - tf::Vector3 p(x,y,z); + tf::Vector3 p(x, y, z); tf::Quaternion q; - q.setRPY(rx,ry,rz); + q.setRPY(rx, ry, rz); - return tf::Transform(q,p); + return tf::Transform(q, p); } // Scan Config Widget -godel_plugins::RobotScanConfigWidget::RobotScanConfigWidget(const godel_msgs::RobotScanParameters& params) - : params_(params) +godel_plugins::RobotScanConfigWidget::RobotScanConfigWidget( + const godel_msgs::RobotScanParameters& params) + : params_(params) { ui_.setupUi(this); @@ -91,19 +91,19 @@ void godel_plugins::RobotScanConfigWidget::update_gui_fields() void godel_plugins::RobotScanConfigWidget::update_internal_values() { params_.num_scan_points = ui_.SpinBoxNumScans->value(); - params_.cam_tilt_angle= DEG2RAD(ui_.LineEditCamTilt->text().toDouble()); - params_.cam_to_obj_xoffset= ui_.LineEditCameraXoffset->text().toDouble(); - params_.cam_to_obj_zoffset= ui_.LineEditCameraZoffset->text().toDouble(); - params_.sweep_angle_start= DEG2RAD(ui_.LineEditSweepAngleStart->text().toDouble()); - params_.sweep_angle_end= DEG2RAD(ui_.LineEditSweepAngleEnd->text().toDouble()); + params_.cam_tilt_angle = DEG2RAD(ui_.LineEditCamTilt->text().toDouble()); + params_.cam_to_obj_xoffset = ui_.LineEditCameraXoffset->text().toDouble(); + params_.cam_to_obj_zoffset = ui_.LineEditCameraZoffset->text().toDouble(); + params_.sweep_angle_start = DEG2RAD(ui_.LineEditSweepAngleStart->text().toDouble()); + params_.sweep_angle_end = DEG2RAD(ui_.LineEditSweepAngleEnd->text().toDouble()); params_.reachable_scan_points_ratio = ui_.LineEditReachablePointRatio->text().toDouble(); - params_.scan_topic= ui_.LineEditScanTopic->text().toStdString(); - params_.scan_target_frame= ui_.LineEditScanTargetFrame->text().toStdString(); - params_.world_frame= ui_.LineEditWorldFrame->text().toStdString(); - params_.tcp_frame= ui_.LineEditTcpFrame->text().toStdString(); - params_.group_name= ui_.LineEditGroupName->text().toStdString(); - params_.stop_on_planning_error= ui_.CheckBoxStopOnPlanningError->isChecked(); + params_.scan_topic = ui_.LineEditScanTopic->text().toStdString(); + params_.scan_target_frame = ui_.LineEditScanTargetFrame->text().toStdString(); + params_.world_frame = ui_.LineEditWorldFrame->text().toStdString(); + params_.tcp_frame = ui_.LineEditTcpFrame->text().toStdString(); + params_.group_name = ui_.LineEditGroupName->text().toStdString(); + params_.stop_on_planning_error = ui_.CheckBoxStopOnPlanningError->isChecked(); - tf::poseTFToMsg(world_to_obj_pose_widget_->get_values(),params_.world_to_obj_pose); - tf::poseTFToMsg(tcp_to_cam_pose_widget_->get_values(),params_.tcp_to_cam_pose); + tf::poseTFToMsg(world_to_obj_pose_widget_->get_values(), params_.world_to_obj_pose); + tf::poseTFToMsg(tcp_to_cam_pose_widget_->get_values(), params_.tcp_to_cam_pose); } diff --git a/godel_plugins/src/widgets/scan_tool_configuration_window.cpp b/godel_plugins/src/widgets/scan_tool_configuration_window.cpp index f778877b..64c53f03 100644 --- a/godel_plugins/src/widgets/scan_tool_configuration_window.cpp +++ b/godel_plugins/src/widgets/scan_tool_configuration_window.cpp @@ -1,7 +1,8 @@ #include "godel_plugins/widgets/scan_tool_configuration_window.h" -godel_plugins::ScanPlanConfigWidget::ScanPlanConfigWidget(const godel_msgs::ScanPlanParameters& params) - : params_(params) +godel_plugins::ScanPlanConfigWidget::ScanPlanConfigWidget( + const godel_msgs::ScanPlanParameters& params) + : params_(params) { ui_.setupUi(this); @@ -11,12 +12,11 @@ godel_plugins::ScanPlanConfigWidget::ScanPlanConfigWidget(const godel_msgs::Scan // Hard Coded Menu Options QStringList quality_metric_list; - quality_metric_list << "rms"; - + quality_metric_list << "rms"; + ui_.ComboBoxQualityMetric->addItems(quality_metric_list); } - void godel_plugins::ScanPlanConfigWidget::update_gui_fields() { ui_.LineEditScanWidth->setText(QString::number(params_.scan_width)); @@ -44,7 +44,6 @@ void godel_plugins::ScanPlanConfigWidget::update_internal_values() params_.window_width = ui_.LineEditWindowWidth->text().toDouble(); params_.min_qa_value = ui_.LineEditMinQAValue->text().toDouble(); params_.max_qa_value = ui_.LineEditMaxQAValue->text().toDouble(); - } int godel_plugins::ScanPlanConfigWidget::get_quality_combobox_index() @@ -58,7 +57,6 @@ int godel_plugins::ScanPlanConfigWidget::get_quality_combobox_index() } } - int godel_plugins::ScanPlanConfigWidget::get_scan_method_enum_value() { switch (ui_.ComboBoxQualityMetric->currentIndex()) diff --git a/godel_plugins/src/widgets/surface_detection_configuration.cpp b/godel_plugins/src/widgets/surface_detection_configuration.cpp index 3ffef5c1..893fccf3 100644 --- a/godel_plugins/src/widgets/surface_detection_configuration.cpp +++ b/godel_plugins/src/widgets/surface_detection_configuration.cpp @@ -8,8 +8,9 @@ #define RAD2DEG(x) ((x)*57.29578) #endif -godel_plugins::SurfaceDetectionConfigWidget::SurfaceDetectionConfigWidget(const godel_msgs::SurfaceDetectionParameters& params) - : params_(params) +godel_plugins::SurfaceDetectionConfigWidget::SurfaceDetectionConfigWidget( + const godel_msgs::SurfaceDetectionParameters& params) + : params_(params) { ui_.setupUi(this); @@ -30,11 +31,13 @@ void godel_plugins::SurfaceDetectionConfigWidget::update_gui_fields() ui_.LineEditRgMinClusterSize->setText(QString::number(params_.rg_min_cluster_size)); ui_.LineEditRgMaxClusterSize->setText(QString::number(params_.rg_max_cluster_size)); ui_.LineEditRgNeighbors->setText(QString::number(params_.rg_neightbors)); - ui_.LineEditRgSmoothnessThreshold->setText(QString::number(RAD2DEG(params_.rg_smoothness_threshold))); + ui_.LineEditRgSmoothnessThreshold->setText( + QString::number(RAD2DEG(params_.rg_smoothness_threshold))); ui_.LineEditRgCurvatureThreshold->setText(QString::number(params_.rg_curvature_threshold)); ui_.LineEditVoxelLeaf->setText(QString::number(params_.voxel_leafsize)); - ui_.LineEditTabletopSegmentationDist->setText(QString::number(params_.tabletop_seg_distance_threshold)); + ui_.LineEditTabletopSegmentationDist->setText( + QString::number(params_.tabletop_seg_distance_threshold)); ui_.CheckBoxUseTabletopSegmentation->setChecked(static_cast(params_.use_tabletop_seg)); ui_.CheckBoxIgnoreLargestCluster->setChecked(static_cast(params_.ignore_largest_cluster)); @@ -45,9 +48,9 @@ void godel_plugins::SurfaceDetectionConfigWidget::update_gui_fields() ui_.LineEditTrSearchRadius->setText(QString::number(params_.tr_search_radius)); ui_.LineEditTrMu->setText(QString::number(params_.tr_mu)); ui_.LineEditTrNearestNeighbors->setText(QString::number(params_.tr_max_nearest_neighbors)); - ui_.LineEditTrMaxSurfaceAngle->setText(QString::number(RAD2DEG( params_.tr_max_surface_angle))); - ui_.LineEditTrMinAngle->setText(QString::number(RAD2DEG( params_.tr_min_angle))); - ui_.LineEditTrMaxAngle->setText(QString::number(RAD2DEG( params_.tr_max_angle))); + ui_.LineEditTrMaxSurfaceAngle->setText(QString::number(RAD2DEG(params_.tr_max_surface_angle))); + ui_.LineEditTrMinAngle->setText(QString::number(RAD2DEG(params_.tr_min_angle))); + ui_.LineEditTrMaxAngle->setText(QString::number(RAD2DEG(params_.tr_max_angle))); ui_.CheckBoxTrNormalConsistency->setChecked(static_cast(params_.tr_normal_consistency)); ui_.CheckBoxPaEnabled->setChecked(static_cast(params_.pa_enabled)); @@ -84,7 +87,7 @@ void godel_plugins::SurfaceDetectionConfigWidget::update_internal_values() params_.tr_search_radius = ui_.LineEditTrSearchRadius->text().toDouble(); params_.tr_mu = ui_.LineEditTrMu->text().toDouble(); params_.tr_max_nearest_neighbors = ui_.LineEditTrNearestNeighbors->text().toDouble(); - params_.tr_max_surface_angle = DEG2RAD( ui_.LineEditTrMaxSurfaceAngle->text().toDouble()); + params_.tr_max_surface_angle = DEG2RAD(ui_.LineEditTrMaxSurfaceAngle->text().toDouble()); params_.tr_min_angle = DEG2RAD(ui_.LineEditTrMinAngle->text().toDouble()); params_.tr_max_angle = DEG2RAD(ui_.LineEditTrMaxAngle->text().toDouble()); params_.tr_normal_consistency = ui_.CheckBoxTrNormalConsistency->isChecked(); diff --git a/godel_polygon_offset/include/godel_polygon_offset/polygon_offset.h b/godel_polygon_offset/include/godel_polygon_offset/polygon_offset.h index 1fbb36cf..e5928078 100644 --- a/godel_polygon_offset/include/godel_polygon_offset/polygon_offset.h +++ b/godel_polygon_offset/include/godel_polygon_offset/polygon_offset.h @@ -34,10 +34,8 @@ #include #include "godel_process_path_generation/polygon_pts.hpp" - using godel_process_path::PolygonBoundaryCollection; - namespace godel_polygon_offset { @@ -45,50 +43,54 @@ namespace godel_polygon_offset * Can handle multiple boundaries of both inside and outside types. * offset_/initial_offset_ are used as offset distances. * Both line segments and arcs are discretized to line segments. - * Resulting polygons are sorted for easy machining: Start at innermost and work outwards, then hop to next innermost. + * Resulting polygons are sorted for easy machining: Start at innermost and work outwards, then hop + * to next innermost. */ class PolygonOffset { public: - PolygonOffset(): verbose_(false), init_ok_(false), - offset_(0.), initial_offset_(0.), discretization_(std::numeric_limits::max()) - {}; - virtual ~PolygonOffset() {}; + PolygonOffset() + : verbose_(false), init_ok_(false), offset_(0.), initial_offset_(0.), + discretization_(std::numeric_limits::max()){}; + virtual ~PolygonOffset(){}; /**@brief Initialize voronoi diagram with polygon points * @param pbc Collection of polygons. CCW(+) ordered points are external boundaries. * @return True if voronoi diagram created successfully. */ - bool init(const PolygonBoundaryCollection &pbc, double _offset, double _initial_offset, double _discretization); + bool init(const PolygonBoundaryCollection& pbc, double _offset, double _initial_offset, + double _discretization); /**@brief Perform offsets on polygons and arrange in a logical order. - * Order of polygons is convenient for godel sanding. Smallest path grows outwards, then jumps to next smallest path. + * Order of polygons is convenient for godel sanding. Smallest path grows outwards, then jumps to + * next smallest path. * @param pbc Resultant polygons. * @param offsets List of offset distances corresponding to polygons. * @return True if successfully performed offsets. */ - bool generateOrderedOffsets(PolygonBoundaryCollection &pbc, std::vector &offsets); + bool generateOrderedOffsets(PolygonBoundaryCollection& pbc, std::vector& offsets); - bool verbose_; /** vd_; - - bool init_ok_; /** vd_; + bool init_ok_; /** #include "godel_polygon_offset/polygon_offset.h" - using godel_process_path::PolygonBoundaryCollection; using godel_process_path::PolygonBoundary; using godel_process_path::PolygonPt; @@ -44,11 +43,11 @@ using godel_process_path::utils::moveItemFrom; typedef std::vector MachiningLoopList; - namespace godel_polygon_offset { -bool PolygonOffset::init(const PolygonBoundaryCollection &pbc, double _offset, double _initial_offset, double _discretization) +bool PolygonOffset::init(const PolygonBoundaryCollection& pbc, double _offset, + double _initial_offset, double _discretization) { if (_offset <= 0. || _initial_offset <= 0. || _discretization <= 0) { @@ -57,7 +56,7 @@ bool PolygonOffset::init(const PolygonBoundaryCollection &pbc, double _offset, d } // Reset PolygonOffset for new operations. - vd_.reset(new ovd::VoronoiDiagram(1,100)); + vd_.reset(new ovd::VoronoiDiagram(1, 100)); offset_ = _offset; initial_offset_ = _initial_offset; discretization_ = _discretization; @@ -66,11 +65,12 @@ bool PolygonOffset::init(const PolygonBoundaryCollection &pbc, double _offset, d // Add all points to vd. std::vector > pt_id_collection; PolygonBoundaryCollection::const_iterator boundary, bs_end; - for (boundary=pbc.begin(), bs_end=pbc.end(); boundary!=bs_end; ++boundary) + for (boundary = pbc.begin(), bs_end = pbc.end(); boundary != bs_end; ++boundary) { std::vector pt_id; - bool first=true; - for (PolygonBoundary::const_iterator pt=boundary->begin(), b_end=boundary->end(); pt!=b_end; ++pt) + bool first = true; + for (PolygonBoundary::const_iterator pt = boundary->begin(), b_end = boundary->end(); + pt != b_end; ++pt) { pt_id.push_back(vd_->insert_point_site(ovd::Point(pt->x, pt->y))); ROS_INFO_COND(verbose_, "Added point %i at location %f, %f", pt_id.back(), pt->x, pt->y); @@ -79,13 +79,13 @@ bool PolygonOffset::init(const PolygonBoundaryCollection &pbc, double _offset, d } // Connect points into boundaries. - for (boundary=pbc.begin(); boundary!=bs_end; ++boundary) + for (boundary = pbc.begin(); boundary != bs_end; ++boundary) { - std::vector &pt_id = pt_id_collection.at(boundary-pbc.begin()); - for (size_t ii=0; ii& pt_id = pt_id_collection.at(boundary - pbc.begin()); + for (size_t ii = 0; ii < pt_id.size() - 1; ++ii) { - ROS_INFO_COND(verbose_, "Adding line from pt %i to pt %i", pt_id.at(ii), pt_id.at(ii+1)); - vd_->insert_line_site(pt_id.at(ii), pt_id.at(ii+1)); + ROS_INFO_COND(verbose_, "Adding line from pt %i to pt %i", pt_id.at(ii), pt_id.at(ii + 1)); + vd_->insert_line_site(pt_id.at(ii), pt_id.at(ii + 1)); } ROS_INFO_COND(verbose_, "Closing loop from pt %i to pt %i", pt_id.back(), pt_id.front()); vd_->insert_line_site(pt_id.back(), pt_id.front()); @@ -109,7 +109,8 @@ bool PolygonOffset::init(const PolygonBoundaryCollection &pbc, double _offset, d return init_ok_; } -bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, std::vector &offsets) +bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection& polygons, + std::vector& offsets) { if (!init_ok_) { @@ -122,7 +123,8 @@ bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, ovd::OffsetSorter sorter(g); /* Perform offsets: - * Start with initial_offset, and proceed with offset distance until no further offsets are generated. + * Start with initial_offset, and proceed with offset distance until no further offsets are + * generated. * Add each offset to sorter for subsequent sorting. */ double offset_distance(initial_offset_); size_t loop_count(0); @@ -135,7 +137,8 @@ bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, break; } loop_count += offset_list.size(); - for (ovd::OffsetLoops::const_iterator loop=offset_list.begin(), loops_end=offset_list.end(); loop!=loops_end; ++loop) + for (ovd::OffsetLoops::const_iterator loop = offset_list.begin(), loops_end = offset_list.end(); + loop != loops_end; ++loop) { sorter.add_loop(*loop); } @@ -143,21 +146,22 @@ bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, } if (loop_count == 0) { - ROS_WARN_STREAM("No offsets were generated: " << std::endl << - "Initial Offset: " << initial_offset_ << " (m)"); + ROS_WARN_STREAM("No offsets were generated: " << std::endl + << "Initial Offset: " << initial_offset_ + << " (m)"); return false; } ROS_INFO_COND(verbose_, "Created %li offset loops", loop_count); // Perform initial sort. sorter.sort_loops(); - const ovd::MachiningGraph &mg = sorter.getMachiningGraph(); + const ovd::MachiningGraph& mg = sorter.getMachiningGraph(); - MachiningLoopList ordered_loops, unordered_loops; // For tracking order of loops for machining + MachiningLoopList ordered_loops, unordered_loops; // For tracking order of loops for machining // Populate unordered loops with all graph vertices ovd::MGVertexItr loop, vertex_end; - for (boost::tie(loop, vertex_end) = boost::vertices(mg); loop!=vertex_end; ++loop) + for (boost::tie(loop, vertex_end) = boost::vertices(mg); loop != vertex_end; ++loop) { unordered_loops.push_back(*loop); } @@ -172,7 +176,7 @@ bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, // Find deepest loop double largest_offset(0); ovd::MGVertex deepest_loop; - BOOST_FOREACH(ovd::MGVertex loop_descriptor, unordered_loops) + BOOST_FOREACH (ovd::MGVertex loop_descriptor, unordered_loops) { if (mg[loop_descriptor].offset_distance > largest_offset) { @@ -190,7 +194,8 @@ bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, { if (exists(child, unordered_loops)) { - ROS_INFO_COND(verbose_, "Moving loop at depth %f to ordered list.", mg[child].offset_distance); + ROS_INFO_COND(verbose_, "Moving loop at depth %f to ordered list.", + mg[child].offset_distance); moveItemFrom(unordered_loops, ordered_loops, child); } else @@ -204,24 +209,26 @@ bool PolygonOffset::generateOrderedOffsets(PolygonBoundaryCollection &polygons, // Populate polygons and offsets polygons.clear(); offsets.clear(); - BOOST_FOREACH(ovd::MGVertex loop_descriptor, ordered_loops) + BOOST_FOREACH (ovd::MGVertex loop_descriptor, ordered_loops) { PolygonBoundary polygon; - const ovd::OffsetLoop &loop = mg[loop_descriptor]; + const ovd::OffsetLoop& loop = mg[loop_descriptor]; ovd::OffsetVertex prior_vtx = loop.vertices.front(); - std::list::const_iterator vtx=boost::next(loop.vertices.begin()); - while ( vtx!=loop.vertices.end() ) + std::list::const_iterator vtx = boost::next(loop.vertices.begin()); + while (vtx != loop.vertices.end()) { PolygonPt prior(prior_vtx.p.x, prior_vtx.p.y), current(vtx->p.x, vtx->p.y); std::vector pts; if (vtx->r == -1) { - pts = godel_process_path::utils::geometry::discretizeLinear(prior, current, discretization_); + pts = + godel_process_path::utils::geometry::discretizeLinear(prior, current, discretization_); } else { PolygonPt arc_center(vtx->c.x, vtx->c.y); - pts = godel_process_path::utils::geometry::discretizeArc2D(prior, current, arc_center, !vtx->cw, discretization_); + pts = godel_process_path::utils::geometry::discretizeArc2D(prior, current, arc_center, + !vtx->cw, discretization_); } polygon.insert(polygon.end(), pts.begin(), pts.end()); pts.clear(); diff --git a/godel_polygon_offset/src/polygon_offset_node.cpp b/godel_polygon_offset/src/polygon_offset_node.cpp index 56f34a65..d4d17d65 100644 --- a/godel_polygon_offset/src/polygon_offset_node.cpp +++ b/godel_polygon_offset/src/polygon_offset_node.cpp @@ -36,8 +36,7 @@ using godel_polygon_offset::OffsetPolygonRequest; using godel_polygon_offset::OffsetPolygonResponse; using namespace godel_process_path; - -bool offset_polygons_cb(OffsetPolygonRequest &req, OffsetPolygonResponse &res) +bool offset_polygons_cb(OffsetPolygonRequest& req, OffsetPolygonResponse& res) { godel_polygon_offset::PolygonOffset po; po.verbose_ = true; @@ -48,7 +47,7 @@ bool offset_polygons_cb(OffsetPolygonRequest &req, OffsetPolygonResponse &res) if (req.initial_offset <= 0.) { - req.initial_offset = req.offset_distance; // Default initial offset if unspecified. + req.initial_offset = req.offset_distance; // Default initial offset if unspecified. } if (!po.init(pbc, req.offset_distance, req.initial_offset, req.discretization)) { @@ -65,7 +64,7 @@ bool offset_polygons_cb(OffsetPolygonRequest &req, OffsetPolygonResponse &res) return true; } -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "polygon_offset_node"); ros::NodeHandle nh; diff --git a/godel_process_execution/include/godel_process_execution/abb_blend_process_service.h b/godel_process_execution/include/godel_process_execution/abb_blend_process_service.h index 7539bc4d..98c3c215 100644 --- a/godel_process_execution/include/godel_process_execution/abb_blend_process_service.h +++ b/godel_process_execution/include/godel_process_execution/abb_blend_process_service.h @@ -30,7 +30,6 @@ class AbbBlendProcessService ros::ServiceClient sim_client_; bool j23_coupled_; }; - } #endif // ABB_BLEND_PROCESS_SERVICE_H diff --git a/godel_process_execution/include/godel_process_execution/blend_process_service.h b/godel_process_execution/include/godel_process_execution/blend_process_service.h index 751df6bd..ed4fdc71 100644 --- a/godel_process_execution/include/godel_process_execution/blend_process_service.h +++ b/godel_process_execution/include/godel_process_execution/blend_process_service.h @@ -24,7 +24,6 @@ class BlendProcessService ros::ServiceClient real_client_; ros::ServiceClient sim_client_; }; - } #endif // BLEND_PROCESS_SERVICE_H diff --git a/godel_process_execution/include/godel_process_execution/keyence_process_service.h b/godel_process_execution/include/godel_process_execution/keyence_process_service.h index e0d81e05..7e39d60d 100644 --- a/godel_process_execution/include/godel_process_execution/keyence_process_service.h +++ b/godel_process_execution/include/godel_process_execution/keyence_process_service.h @@ -29,7 +29,6 @@ class KeyenceProcessService ros::ServiceClient sim_client_; ros::ServiceClient keyence_client_; }; - } #endif // KEYENCE_PROCESS_SERVICE_H diff --git a/godel_process_execution/src/abb_blend_process_service.cpp b/godel_process_execution/src/abb_blend_process_service.cpp index ad5043c2..f7c4622c 100644 --- a/godel_process_execution/src/abb_blend_process_service.cpp +++ b/godel_process_execution/src/abb_blend_process_service.cpp @@ -13,22 +13,24 @@ #include "sensor_msgs/JointState.h" #include "ros/topic.h" -const static double DEFAULT_JOINT_TOPIC_WAIT_TIME = 5.0; //seconds -const static double DEFAULT_TRAJECTORY_BUFFER_TIME = 5.0; //seconds +const static double DEFAULT_JOINT_TOPIC_WAIT_TIME = 5.0; // seconds +const static double DEFAULT_TRAJECTORY_BUFFER_TIME = 5.0; // seconds const static std::string JOINT_TOPIC_NAME = "/joint_states"; const static std::string THIS_SERVICE_NAME = "blend_process_execution"; const static std::string EXECUTION_SERVICE_NAME = "execute_program"; const static std::string SIMULATION_SERVICE_NAME = "simulate_path"; -static inline bool compare(const std::vector& a, const std::vector& b, double eps = 0.01) +static inline bool compare(const std::vector& a, const std::vector& b, + double eps = 0.01) { - if (a.size() != b.size()) + if (a.size() != b.size()) { - ROS_ERROR_STREAM("Can't compare joint vectors of unequal length (" << a.size() << " vs " << b.size() << ")"); + ROS_ERROR_STREAM("Can't compare joint vectors of unequal length (" << a.size() << " vs " + << b.size() << ")"); return false; } - + double diff = 0.0; for (std::size_t i = 0; i < a.size(); ++i) { @@ -36,11 +38,12 @@ static inline bool compare(const std::vector& a, const std::vector& end_goal, const ros::Duration& wait_for, const ros::Duration& time_out) +static bool waitForExecution(const std::vector& end_goal, const ros::Duration& wait_for, + const ros::Duration& time_out) { - sensor_msgs::JointStateConstPtr state; + sensor_msgs::JointStateConstPtr state; ros::Time end_time = ros::Time::now() + time_out; // wait a fixed amount of time @@ -48,7 +51,8 @@ static bool waitForExecution(const std::vector& end_goal, const ros::Dur while (ros::Time::now() < end_time) { - state = ros::topic::waitForMessage(JOINT_TOPIC_NAME, ros::Duration(DEFAULT_JOINT_TOPIC_WAIT_TIME)); + state = ros::topic::waitForMessage( + JOINT_TOPIC_NAME, ros::Duration(DEFAULT_JOINT_TOPIC_WAIT_TIME)); if (!state) { ROS_WARN("Could not get a joint_state in time"); @@ -63,10 +67,7 @@ static bool waitForExecution(const std::vector& end_goal, const ros::Dur return false; } -static double toDegrees(double rads) -{ - return rads * 180.0 / M_PI; -} +static double toDegrees(double rads) { return rads * 180.0 / M_PI; } static std::vector toDegrees(const std::vector& rads) { @@ -103,11 +104,11 @@ toRapidTrajectory(const trajectory_msgs::JointTrajectory& traj, bool j23_coupled double duration = 0.0; if (i > 0) { - duration = (traj.points[i].time_from_start - traj.points[i-1].time_from_start).toSec(); + duration = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).toSec(); } // Now we have all the info we need - rapid_emitter::TrajectoryPt rapid_point (angles, duration); + rapid_emitter::TrajectoryPt rapid_point(angles, duration); rapid_pts.push_back(rapid_point); } return rapid_pts; @@ -115,11 +116,10 @@ toRapidTrajectory(const trajectory_msgs::JointTrajectory& traj, bool j23_coupled static bool writeRapidFile(const std::string& path, const std::vector& traj, - unsigned process_start, - unsigned process_stop, + unsigned process_start, unsigned process_stop, const rapid_emitter::ProcessParams& params) { - std::ofstream fp ("/tmp/blend.mod"); + std::ofstream fp("/tmp/blend.mod"); if (!fp) { ROS_ERROR_STREAM("Unable to create file: " << path); @@ -142,19 +142,20 @@ godel_process_execution::AbbBlendProcessService::AbbBlendProcessService(ros::Nod nh.param("J23_coupled", j23_coupled_, false); // Create client services - sim_client_ = nh.serviceClient(SIMULATION_SERVICE_NAME); + sim_client_ = nh.serviceClient( + SIMULATION_SERVICE_NAME); real_client_ = nh.serviceClient(EXECUTION_SERVICE_NAME); // Advertise the primary blending service - server_ = nh.advertiseService - (THIS_SERVICE_NAME, &godel_process_execution::AbbBlendProcessService::executionCallback, this); + server_ = nh.advertiseService( + THIS_SERVICE_NAME, &godel_process_execution::AbbBlendProcessService::executionCallback, this); } -bool godel_process_execution::AbbBlendProcessService::executionCallback(godel_msgs::BlendProcessExecution::Request& req, - godel_msgs::BlendProcessExecution::Response& res) +bool godel_process_execution::AbbBlendProcessService::executionCallback( + godel_msgs::BlendProcessExecution::Request& req, + godel_msgs::BlendProcessExecution::Response& res) { if (req.simulate) { @@ -166,14 +167,15 @@ bool godel_process_execution::AbbBlendProcessService::executionCallback(godel_ms } } -bool godel_process_execution::AbbBlendProcessService::executeProcess(godel_msgs::BlendProcessExecution::Request req) +bool godel_process_execution::AbbBlendProcessService::executeProcess( + godel_msgs::BlendProcessExecution::Request req) { trajectory_msgs::JointTrajectory aggregate_traj; aggregate_traj = req.trajectory_approach; appendTrajectory(aggregate_traj, req.trajectory_process); appendTrajectory(aggregate_traj, req.trajectory_depart); - //ABB Rapid Emmiter + // ABB Rapid Emmiter std::vector pts = toRapidTrajectory(aggregate_traj, j23_coupled_); // RAPID process parameters @@ -210,13 +212,16 @@ bool godel_process_execution::AbbBlendProcessService::executeProcess(godel_msgs: return true; } - // If we must wait for execution, then block and listen until robot returns to initial point or times out + // If we must wait for execution, then block and listen until robot returns to initial point or + // times out return waitForExecution(req.trajectory_approach.points.front().positions, aggregate_traj.points.back().time_from_start, // wait for - aggregate_traj.points.back().time_from_start + ros::Duration(DEFAULT_TRAJECTORY_BUFFER_TIME)); // timeout + aggregate_traj.points.back().time_from_start + + ros::Duration(DEFAULT_TRAJECTORY_BUFFER_TIME)); // timeout } -bool godel_process_execution::AbbBlendProcessService::simulateProcess(godel_msgs::BlendProcessExecution::Request req) +bool godel_process_execution::AbbBlendProcessService::simulateProcess( + godel_msgs::BlendProcessExecution::Request req) { // The simulation server doesn't support any I/O visualizations, so we aggregate the // trajectory components and send them all at once diff --git a/godel_process_execution/src/abb_blend_process_service_node.cpp b/godel_process_execution/src/abb_blend_process_service_node.cpp index fc0d480e..e6ad48c2 100644 --- a/godel_process_execution/src/abb_blend_process_service_node.cpp +++ b/godel_process_execution/src/abb_blend_process_service_node.cpp @@ -7,7 +7,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "abb_blend_process_service_node"); ros::NodeHandle nh; - + godel_process_execution::AbbBlendProcessService process_executor(nh); ros::spin(); diff --git a/godel_process_execution/src/blend_process_service.cpp b/godel_process_execution/src/blend_process_service.cpp index f0d0a62f..5575dffe 100644 --- a/godel_process_execution/src/blend_process_service.cpp +++ b/godel_process_execution/src/blend_process_service.cpp @@ -15,18 +15,19 @@ const static std::string THIS_SERVICE_NAME = "blend_process_execution"; godel_process_execution::BlendProcessService::BlendProcessService(ros::NodeHandle& nh) { // Simulation Server - sim_client_ = nh.serviceClient(SIMULATION_SERVICE_NAME); + sim_client_ = nh.serviceClient( + SIMULATION_SERVICE_NAME); // Trajectory Execution Service real_client_ = nh.serviceClient(EXECUTION_SERVICE_NAME); // The generic process execution service - server_ = nh.advertiseService - (THIS_SERVICE_NAME, &godel_process_execution::BlendProcessService::executionCallback, this); + server_ = nh.advertiseService( + THIS_SERVICE_NAME, &godel_process_execution::BlendProcessService::executionCallback, this); } -bool godel_process_execution::BlendProcessService::executionCallback(godel_msgs::BlendProcessExecution::Request& req, - godel_msgs::BlendProcessExecution::Response& res) +bool godel_process_execution::BlendProcessService::executionCallback( + godel_msgs::BlendProcessExecution::Request& req, + godel_msgs::BlendProcessExecution::Response& res) { if (req.simulate) { @@ -47,7 +48,8 @@ bool godel_process_execution::BlendProcessService::executionCallback(godel_msgs: } } -bool godel_process_execution::BlendProcessService::executeProcess(godel_msgs::BlendProcessExecution::Request req) +bool godel_process_execution::BlendProcessService::executeProcess( + godel_msgs::BlendProcessExecution::Request req) { godel_msgs::TrajectoryExecution srv_approach; srv_approach.request.wait_for_execution = true; @@ -82,7 +84,8 @@ bool godel_process_execution::BlendProcessService::executeProcess(godel_msgs::Bl return true; } -bool godel_process_execution::BlendProcessService::simulateProcess(godel_msgs::BlendProcessExecution::Request req) +bool godel_process_execution::BlendProcessService::simulateProcess( + godel_msgs::BlendProcessExecution::Request req) { using industrial_robot_simulator_service::SimulateTrajectory; diff --git a/godel_process_execution/src/keyence_process_service.cpp b/godel_process_execution/src/keyence_process_service.cpp index 5f118373..046eb8f9 100644 --- a/godel_process_execution/src/keyence_process_service.cpp +++ b/godel_process_execution/src/keyence_process_service.cpp @@ -19,23 +19,24 @@ const static std::string EXECUTION_SERVICE_NAME = "execute_path"; const static std::string SIMULATION_SERVICE_NAME = "simulate_path"; const static std::string SERVICE_SERVER_NAME = "scan_process_execution"; - godel_process_execution::KeyenceProcessService::KeyenceProcessService(ros::NodeHandle& nh) { // Connect to motion servers and I/O server - sim_client_ = nh.serviceClient(SIMULATION_SERVICE_NAME); + sim_client_ = nh.serviceClient( + SIMULATION_SERVICE_NAME); real_client_ = nh.serviceClient(EXECUTION_SERVICE_NAME); // Create this process execution server - server_ = nh.advertiseService - (SERVICE_SERVER_NAME, &godel_process_execution::KeyenceProcessService::executionCallback, this); + server_ = nh.advertiseService( + SERVICE_SERVER_NAME, &godel_process_execution::KeyenceProcessService::executionCallback, + this); } -bool godel_process_execution::KeyenceProcessService::executionCallback(godel_msgs::KeyenceProcessExecution::Request& req, - godel_msgs::KeyenceProcessExecution::Response& res) +bool godel_process_execution::KeyenceProcessService::executionCallback( + godel_msgs::KeyenceProcessExecution::Request& req, + godel_msgs::KeyenceProcessExecution::Response& res) { if (req.simulate) { @@ -55,13 +56,15 @@ bool godel_process_execution::KeyenceProcessService::executionCallback(godel_msg } } -bool godel_process_execution::KeyenceProcessService::executeProcess(godel_msgs::KeyenceProcessExecution::Request& req) +bool godel_process_execution::KeyenceProcessService::executeProcess( + godel_msgs::KeyenceProcessExecution::Request& req) { - #if KEYENCE_DRIVER_IMPLEMENTED +#if KEYENCE_DRIVER_IMPLEMENTED // Check for keyence existence if (!keyence_client_.exists()) { - ROS_ERROR_STREAM("Keyence ROS server is not available on service " << keyence_client_.getService()); + ROS_ERROR_STREAM("Keyence ROS server is not available on service " + << keyence_client_.getService()); return false; } @@ -89,7 +92,7 @@ bool godel_process_execution::KeyenceProcessService::executeProcess(godel_msgs:: if (!keyence_client_.call(keyence_srv)) { ROS_ERROR_STREAM("Unable to activate keyence (program " << KEYENCE_PROGRAM_LASER_ON << ")."); - return false; + return false; } if (!real_client_.call(srv_process)) @@ -102,7 +105,8 @@ bool godel_process_execution::KeyenceProcessService::executeProcess(godel_msgs:: keyence_srv.request.program_no = KEYENCE_PROGRAM_LASER_OFF; if (!keyence_client_.call(keyence_srv)) { - ROS_ERROR_STREAM("Unable to de-activate keyence (program " << KEYENCE_PROGRAM_LASER_OFF << ")."); + ROS_ERROR_STREAM("Unable to de-activate keyence (program " << KEYENCE_PROGRAM_LASER_OFF + << ")."); return false; } @@ -113,14 +117,15 @@ bool godel_process_execution::KeyenceProcessService::executeProcess(godel_msgs:: } return true; - #else +#else ROS_WARN_STREAM("Keyence Driver is not yet implemented."); return false; - #endif +#endif } -bool godel_process_execution::KeyenceProcessService::simulateProcess(godel_msgs::KeyenceProcessExecution::Request& req) +bool godel_process_execution::KeyenceProcessService::simulateProcess( + godel_msgs::KeyenceProcessExecution::Request& req) { using industrial_robot_simulator_service::SimulateTrajectory; diff --git a/godel_process_execution/src/keyence_process_service_node.cpp b/godel_process_execution/src/keyence_process_service_node.cpp index 0f2e117e..e6bc91b3 100644 --- a/godel_process_execution/src/keyence_process_service_node.cpp +++ b/godel_process_execution/src/keyence_process_service_node.cpp @@ -6,7 +6,7 @@ int main(int argc, char** argv) { ros::init(argc, argv, "keyence_process_service_node"); ros::NodeHandle nh; - + godel_process_execution::KeyenceProcessService process_executor(nh); ros::spin(); diff --git a/godel_process_execution/src/process_utils.cpp b/godel_process_execution/src/process_utils.cpp index 97beddb9..9536242a 100644 --- a/godel_process_execution/src/process_utils.cpp +++ b/godel_process_execution/src/process_utils.cpp @@ -1,10 +1,11 @@ #include "process_utils.h" -void godel_process_execution::appendTrajectory(trajectory_msgs::JointTrajectory& original, - const trajectory_msgs::JointTrajectory& next) +void godel_process_execution::appendTrajectory(trajectory_msgs::JointTrajectory& original, + const trajectory_msgs::JointTrajectory& next) { - ros::Duration last_t = original.points.empty() ? ros::Duration(0.0) : original.points.back().time_from_start; - for (std::size_t i = 0 ; i < next.points.size(); ++i) + ros::Duration last_t = + original.points.empty() ? ros::Duration(0.0) : original.points.back().time_from_start; + for (std::size_t i = 0; i < next.points.size(); ++i) { trajectory_msgs::JointTrajectoryPoint pt = next.points[i]; pt.time_from_start += last_t; diff --git a/godel_process_execution/src/process_utils.h b/godel_process_execution/src/process_utils.h index adfebe4d..8c3d5f12 100644 --- a/godel_process_execution/src/process_utils.h +++ b/godel_process_execution/src/process_utils.h @@ -6,7 +6,7 @@ namespace godel_process_execution { -void appendTrajectory(trajectory_msgs::JointTrajectory& original, +void appendTrajectory(trajectory_msgs::JointTrajectory& original, const trajectory_msgs::JointTrajectory& next); } diff --git a/godel_process_path_generation/include/godel_process_path_generation/get_boundary.h b/godel_process_path_generation/include/godel_process_path_generation/get_boundary.h index 707563b5..80a3676b 100644 --- a/godel_process_path_generation/include/godel_process_path_generation/get_boundary.h +++ b/godel_process_path_generation/include/godel_process_path_generation/get_boundary.h @@ -45,51 +45,54 @@ namespace pcl_godel { - namespace geometry - { - /** \brief Get a collection of boundary half-edges for the input mesh. - * \param[in] mesh The input mesh. - * \param[out] boundary_he_collection Collection of boundary half-edges. Each element in the vector is one connected boundary. The whole boundary is the union of all elements. - * \param [in] expected_size If you already know the size of the longest boundary you can tell this here. Defaults to 3 (minimum possible boundary). - * \author Martin Saelzle - * \ingroup geometry - */ - template void - getBoundBoundaryHalfEdges (const MeshT& mesh, - std::vector & boundary_he_collection, - const size_t expected_size = 3) - { - typedef MeshT Mesh; - typedef typename Mesh::HalfEdgeIndex HalfEdgeIndex; - typedef typename Mesh::HalfEdgeIndices HalfEdgeIndices; - typedef typename Mesh::InnerHalfEdgeAroundFaceCirculator IHEAFC; +namespace geometry +{ +/** \brief Get a collection of boundary half-edges for the input mesh. + * \param[in] mesh The input mesh. + * \param[out] boundary_he_collection Collection of boundary half-edges. Each element in the vector + * is one connected boundary. The whole boundary is the union of all elements. + * \param [in] expected_size If you already know the size of the longest boundary you can tell this + * here. Defaults to 3 (minimum possible boundary). + * \author Martin Saelzle + * \ingroup geometry + */ +template +void getBoundBoundaryHalfEdges(const MeshT& mesh, + std::vector& boundary_he_collection, + const size_t expected_size = 3) +{ + typedef MeshT Mesh; + typedef typename Mesh::HalfEdgeIndex HalfEdgeIndex; + typedef typename Mesh::HalfEdgeIndices HalfEdgeIndices; + typedef typename Mesh::InnerHalfEdgeAroundFaceCirculator IHEAFC; - boundary_he_collection.clear (); + boundary_he_collection.clear(); - HalfEdgeIndices boundary_he; boundary_he.reserve (expected_size); - std::vector visited (mesh.sizeEdges (), false); - IHEAFC circ, circ_end; + HalfEdgeIndices boundary_he; + boundary_he.reserve(expected_size); + std::vector visited(mesh.sizeEdges(), false); + IHEAFC circ, circ_end; - for (HalfEdgeIndex i (0); i #include "godel_process_path_generation/polygon_pts.hpp" - namespace godel_process_path { typedef pcl::PointCloud Cloud; - - class MeshImporter { public: - MeshImporter(bool verbose=false): verbose_(verbose) {}; - virtual ~MeshImporter() {}; + MeshImporter(bool verbose = false) : verbose_(verbose){}; + virtual ~MeshImporter(){}; - /**@brief Create local coordinate system and boundary data for a point cloud representing a flat surface + /**@brief Create local coordinate system and boundary data for a point cloud representing a flat + * surface * Note: Boundary data given in local frame of point cloud - * @param input_mesh PolygonMesh msg containing binary point cloud data and triagonalized mesh data + * @param input_mesh PolygonMesh msg containing binary point cloud data and triagonalized mesh + * data * @return true if calculations are successful */ - bool calculateBoundaryData(const pcl::PolygonMesh &input_mesh); + bool calculateBoundaryData(const pcl::PolygonMesh& input_mesh); - bool calculateSimpleBoundary(const pcl::PolygonMesh &input_mesh); + bool calculateSimpleBoundary(const pcl::PolygonMesh& input_mesh); /**@brief Get const reference to the boundary data */ - const PolygonBoundaryCollection& getBoundaries() const - { - return boundaries_; - } + const PolygonBoundaryCollection& getBoundaries() const { return boundaries_; } - void getPose(geometry_msgs::Pose& pose) - { - tf::poseEigenToMsg(plane_frame_,pose); - } + void getPose(geometry_msgs::Pose& pose) { tf::poseEigenToMsg(plane_frame_, pose); } - bool verbose_; /**< @brief Flag to print additional information */ + bool verbose_; /**< @brief Flag to print additional information */ private: + bool applyConcaveHull(const Cloud& plane_cloud, pcl::ModelCoefficients& plane_coeffs, + geometry_msgs::PolygonStamped& polygon); - bool applyConcaveHull(const Cloud& plane_cloud,pcl::ModelCoefficients &plane_coeffs,geometry_msgs::PolygonStamped& polygon); - - void computeLocalPlaneFrame(const Eigen::Hyperplane &plane, const Eigen::Vector4d ¢roid, const Cloud& cloud); + void computeLocalPlaneFrame(const Eigen::Hyperplane& plane, + const Eigen::Vector4d& centroid, const Cloud& cloud); /**@brief Compute coefficients of a plane best fit to point cloud * Coefficients correspond to ax+by+cz+d=0 @@ -83,12 +77,11 @@ class MeshImporter * @param output Coefficients * @return False if plane fit poor, True otherwise. */ - bool computePlaneCoefficients(Cloud::ConstPtr cloud, Eigen::Vector4d &output); - - - Eigen::Affine3d plane_frame_; /**< @brief Transform to local frame of plane */ - PolygonBoundaryCollection boundaries_; /**< @brief List of boundaries. External boundary must be ordered CCW, internal CW */ + bool computePlaneCoefficients(Cloud::ConstPtr cloud, Eigen::Vector4d& output); + Eigen::Affine3d plane_frame_; /**< @brief Transform to local frame of plane */ + PolygonBoundaryCollection boundaries_; /**< @brief List of boundaries. External boundary must be + ordered CCW, internal CW */ }; } /* namespace godel_process_path */ diff --git a/godel_process_path_generation/include/godel_process_path_generation/polygon_pts.hpp b/godel_process_path_generation/include/godel_process_path_generation/polygon_pts.hpp index 452ba5b6..db48ca4e 100644 --- a/godel_process_path_generation/include/godel_process_path_generation/polygon_pts.hpp +++ b/godel_process_path_generation/include/godel_process_path_generation/polygon_pts.hpp @@ -37,29 +37,38 @@ namespace godel_process_path /**@brief Represents a point on the boundary of a 2D polygon */ struct PolygonPt { - PolygonPt() {}; - PolygonPt(double _x, double _y): x(_x), y(_y) {}; + PolygonPt(){}; + PolygonPt(double _x, double _y) : x(_x), y(_y){}; double x; double y; - inline bool operator==(const PolygonPt& other) const {return x==other.x && y==other.y;} - inline bool operator!=(const PolygonPt& other) const {return !operator==(other);} - inline PolygonPt operator-(const PolygonPt& other) const {return PolygonPt(x-other.x, y-other.y);} - inline PolygonPt operator+(const PolygonPt& other) const {return PolygonPt(x+other.x, y+other.y);} - inline PolygonPt operator*(double d) const {return PolygonPt(x*d, y*d);} - inline PolygonPt operator/(double d) const {return (*this)*1./d;} + inline bool operator==(const PolygonPt& other) const { return x == other.x && y == other.y; } + inline bool operator!=(const PolygonPt& other) const { return !operator==(other); } + inline PolygonPt operator-(const PolygonPt& other) const + { + return PolygonPt(x - other.x, y - other.y); + } + inline PolygonPt operator+(const PolygonPt& other) const + { + return PolygonPt(x + other.x, y + other.y); + } + inline PolygonPt operator*(double d) const { return PolygonPt(x * d, y * d); } + inline PolygonPt operator/(double d) const { return (*this) * 1. / d; } - inline double dist2(const PolygonPt &pt) const {return (pt.x-x)*(pt.x-x) + (pt.y-y)*(pt.y-y);} - inline double dist(const PolygonPt &pt) const {return std::sqrt(dist2(pt));} - inline double norm2() const {return x*x + y*y;} - inline double norm() const {return std::sqrt(norm2());} - inline double dot(const PolygonPt &pt) const {return x*pt.x + y*pt.y;} - inline double cross(const PolygonPt &pt) const {return x*pt.y - y*pt.x;} + inline double dist2(const PolygonPt& pt) const + { + return (pt.x - x) * (pt.x - x) + (pt.y - y) * (pt.y - y); + } + inline double dist(const PolygonPt& pt) const { return std::sqrt(dist2(pt)); } + inline double norm2() const { return x * x + y * y; } + inline double norm() const { return std::sqrt(norm2()); } + inline double dot(const PolygonPt& pt) const { return x * pt.x + y * pt.y; } + inline double cross(const PolygonPt& pt) const { return x * pt.y - y * pt.x; } - friend ostream& operator<<(ostream &out, const PolygonPt &ppt); + friend ostream& operator<<(ostream& out, const PolygonPt& ppt); }; -inline ostream& operator<<(ostream &out, const PolygonPt &ppt) +inline ostream& operator<<(ostream& out, const PolygonPt& ppt) { out << "(" << ppt.x << ", " << ppt.y << ")" << std::endl; return out; @@ -68,9 +77,9 @@ inline ostream& operator<<(ostream &out, const PolygonPt &ppt) /**@brief A collection of polygon points that represent the boundary of a polygon. * Note: Last point assumed to join to 1st pt, not repeated in data. */ typedef std::vector PolygonBoundary; -inline ostream& operator<<(ostream &out, const PolygonBoundary &pb) +inline ostream& operator<<(ostream& out, const PolygonBoundary& pb) { - for (PolygonBoundary::const_iterator pt=pb.begin(), pt_end=pb.end(); pt != pt_end; ++pt) + for (PolygonBoundary::const_iterator pt = pb.begin(), pt_end = pb.end(); pt != pt_end; ++pt) { out << *pt; } @@ -79,12 +88,14 @@ inline ostream& operator<<(ostream &out, const PolygonBoundary &pb) /**@brief A collection of polygonal boundaries that make up a complex shape */ typedef std::vector PolygonBoundaryCollection; -inline ostream& operator<<(ostream &out, const PolygonBoundaryCollection &pbc) +inline ostream& operator<<(ostream& out, const PolygonBoundaryCollection& pbc) { size_t idx(0); - for (PolygonBoundaryCollection::const_iterator pb=pbc.begin(), pb_end=pbc.end(); pb != pb_end; ++pb, ++idx) + for (PolygonBoundaryCollection::const_iterator pb = pbc.begin(), pb_end = pbc.end(); pb != pb_end; + ++pb, ++idx) { - out << "Polygon " << idx << std::endl << *pb; + out << "Polygon " << idx << std::endl + << *pb; } return out; } diff --git a/godel_process_path_generation/include/godel_process_path_generation/polygon_utils.h b/godel_process_path_generation/include/godel_process_path_generation/polygon_utils.h index 3d7207e9..e8027b60 100644 --- a/godel_process_path_generation/include/godel_process_path_generation/polygon_utils.h +++ b/godel_process_path_generation/include/godel_process_path_generation/polygon_utils.h @@ -29,7 +29,6 @@ #include #include "godel_process_path_generation/polygon_pts.hpp" - namespace godel_process_path { namespace polygon_utils @@ -37,40 +36,41 @@ namespace polygon_utils struct PolygonSegment { - PolygonSegment(const PolygonPt &p1, const PolygonPt &p2): start(p1), end(p2) {}; + PolygonSegment(const PolygonPt& p1, const PolygonPt& p2) : start(p1), end(p2){}; PolygonPt start; PolygonPt end; /**@brief Does this segment intersect another * Does not account for segments that are nearly parallel * @param tol Tolerance on how close endpoints can be to line before considered touching */ - bool intersects(const PolygonSegment &other, double tol=1e-5) const; - inline double length() const {return start.dist(end);}; - inline double length2() const {return start.dist2(end);}; - inline PolygonPt vector() const {return end-start;}; - inline double cross(const PolygonSegment &other) const {return vector().cross(other.vector());}; + bool intersects(const PolygonSegment& other, double tol = 1e-5) const; + inline double length() const { return start.dist(end); }; + inline double length2() const { return start.dist2(end); }; + inline PolygonPt vector() const { return end - start; }; + inline double cross(const PolygonSegment& other) const { return vector().cross(other.vector()); }; }; -void boundaryToSegments(std::vector &segments, const PolygonBoundary &polygon); +void boundaryToSegments(std::vector& segments, const PolygonBoundary& polygon); /**@brief Checks that PolygonBoundary is valid (non-self-intersecting) * @param bnd Polygon to check * @return True if polygon has no self-intersections */ -bool checkBoundary(const PolygonBoundary &bnd); +bool checkBoundary(const PolygonBoundary& bnd); /**@brief Checks that PolygonBoundaryCollection is valid * Checks for self-intersection of each polygon, and global intersections of polygons * @param pbc PolygonBoundaryCollection to check * @return False if any intersection check is invalid, True otherwise */ -bool checkBoundaryCollection(const PolygonBoundaryCollection &pbc); +bool checkBoundaryCollection(const PolygonBoundaryCollection& pbc); /**@brief calculate total length of boundary by summing segments */ -inline double circumference(const PolygonBoundary &boundary) +inline double circumference(const PolygonBoundary& boundary) { - double dist(0); // Total distance squared - for (PolygonBoundary::const_iterator pt=boundary.begin(); pt!=boost::prior(boundary.end()); ++pt) + double dist(0); // Total distance squared + for (PolygonBoundary::const_iterator pt = boundary.begin(); pt != boost::prior(boundary.end()); + ++pt) { dist += pt->dist(*boost::next(pt)); } @@ -83,21 +83,23 @@ inline double circumference(const PolygonBoundary &boundary) * @param bnd PolygonBoundary to find closest point in * @return pair(index into PolygonBoundary, distance) */ -std::pair closestPoint(const PolygonPt &pt, const PolygonBoundary &bnd); +std::pair closestPoint(const PolygonPt& pt, const PolygonBoundary& bnd); /**@brief Downsample boundary so that edges are represented by endpoints * Points may be removed from boundary to satisfy this filter * @param boundary PolygonBoundary to modify - * @param tol allowable angle between adjacent segments to decide if edge is linear. If tol is outside [0, pi/2] it is reset to closest bound. + * @param tol allowable angle between adjacent segments to decide if edge is linear. If tol is + * outside [0, pi/2] it is reset to closest bound. */ -void filter(PolygonBoundary &boundary, double tol); +void filter(PolygonBoundary& boundary, double tol); /**@brief Downsample all boundaries so that edges are represented by endpoints * Points may be removed from boundaries to satisfy this filter * @param boundaries PolygonBoundaryCollection to modify - * @param tol allowable angle between adjacent segments to decide if edge is linear. If tol is outside [0, pi/2] it is reset to closest bound. + * @param tol allowable angle between adjacent segments to decide if edge is linear. If tol is + * outside [0, pi/2] it is reset to closest bound. */ -inline void filter(PolygonBoundaryCollection &boundaries, double tol) +inline void filter(PolygonBoundaryCollection& boundaries, double tol) { // Check bounds of tol. if (tol > M_PI_2) @@ -111,17 +113,17 @@ inline void filter(PolygonBoundaryCollection &boundaries, double tol) tol = 0.; } - BOOST_FOREACH(PolygonBoundary &boundary, boundaries) + BOOST_FOREACH (PolygonBoundary& boundary, boundaries) { filter(boundary, tol); } } /**@brief Check if a intersects b */ -bool intersects(const PolygonBoundary &a, const PolygonBoundary &b); +bool intersects(const PolygonBoundary& a, const PolygonBoundary& b); /**@brief Check if a intersects b */ -bool intersects(const std::vector &a, const std::vector &b); +bool intersects(const std::vector& a, const std::vector& b); } /* namespace polygon_utils */ } /* namespace godel_process_path */ diff --git a/godel_process_path_generation/include/godel_process_path_generation/process_path.h b/godel_process_path_generation/include/godel_process_path_generation/process_path.h index 00166435..bd423865 100644 --- a/godel_process_path_generation/include/godel_process_path_generation/process_path.h +++ b/godel_process_path_generation/include/godel_process_path_generation/process_path.h @@ -35,22 +35,24 @@ namespace descartes class ProcessPath { public: - ProcessPath() {}; - virtual ~ProcessPath() {}; + ProcessPath(){}; + virtual ~ProcessPath(){}; /**@brief Add a ProcessPt to pts * @param pt Reference to ProcessPt to add to points */ - void addPoint(const ProcessPt &pt) {pts_.push_back(pt);} + void addPoint(const ProcessPt& pt) { pts_.push_back(pt); } /**@brief Add a ProcessTranstition to transition_ * @param trans Reference to ProcessTransition to add to points */ - void addTransition(const ProcessTransition &trans) {transitions_.push_back(trans);} + void addTransition(const ProcessTransition& trans) { transitions_.push_back(trans); } /**@brief Get copy of data */ - std::pair, std::vector > data() const { - return std::make_pair(pts_, transitions_); } + std::pair, std::vector > data() const + { + return std::make_pair(pts_, transitions_); + } /**@brief Convert ProcessPath to line_list marker * Does not populate header, ns, id, lifetime, frame_locked @@ -58,8 +60,11 @@ class ProcessPath */ visualization_msgs::Marker asMarker() const; - void clear() {pts_.clear(); transitions_.clear();} - + void clear() + { + pts_.clear(); + transitions_.clear(); + } protected: std::vector pts_; diff --git a/godel_process_path_generation/include/godel_process_path_generation/process_path_generator.h b/godel_process_path_generation/include/godel_process_path_generation/process_path_generator.h index e0390ffb..bb9c5ab6 100644 --- a/godel_process_path_generation/include/godel_process_path_generation/process_path_generator.h +++ b/godel_process_path_generation/include/godel_process_path_generation/process_path_generator.h @@ -29,7 +29,6 @@ #include "godel_process_path_generation/process_path.h" #include "godel_process_path_generation/polygon_utils.h" - using descartes::ProcessPt; using descartes::ProcessPath; @@ -38,29 +37,28 @@ namespace godel_process_path struct ProcessVelocity { - ProcessVelocity(): approach(0.), blending(0.), retract(0.), traverse(0.) {}; - ProcessVelocity(double a, double b, double r, double t): approach(a), blending(b), retract(r), traverse(t) {}; + ProcessVelocity() : approach(0.), blending(0.), retract(0.), traverse(0.){}; + ProcessVelocity(double a, double b, double r, double t) + : approach(a), blending(b), retract(r), traverse(t){}; double approach, blending, retract, traverse; }; - class ProcessPathGenerator { public: - ProcessPathGenerator(): tool_radius_(0.), margin_(0.), overlap_(0.), safe_traverse_height_(-1.), - verbose_(false) - {}; - virtual ~ProcessPathGenerator() {}; + ProcessPathGenerator() + : tool_radius_(0.), margin_(0.), overlap_(0.), safe_traverse_height_(-1.), verbose_(false){}; + virtual ~ProcessPathGenerator(){}; bool createProcessPath(); - const descartes::ProcessPath& getProcessPath() const {return process_path_;} + const descartes::ProcessPath& getProcessPath() const { return process_path_; } /**@brief Set data used by path generator. * Note: Polygons data may become rotated during processing, but order will remain consistent. * @param polygons Pointer to collection of polygons that will be joined into ProcessPath * @param offset_depths Offset distance of each polygon */ - bool setPathPolygons(PolygonBoundaryCollection *polygons, std::vector *offset_depths) + bool setPathPolygons(PolygonBoundaryCollection* polygons, std::vector* offset_depths) { if (!polygon_utils::checkBoundaryCollection(*polygons)) { @@ -73,60 +71,61 @@ class ProcessPathGenerator return true; } - void setDiscretizationDistance(double d) {max_discretization_distance_ = std::abs(d);} - void setMargin(double margin) {margin_=margin;} - void setOverlap(double overlap) {overlap_=overlap;} - void setToolRadius(double radius) {tool_radius_=std::abs(radius);} - void setTraverseHeight(double height) {safe_traverse_height_=height;} - void setVelocity(const ProcessVelocity &vel) {velocity_=vel;} + void setDiscretizationDistance(double d) { max_discretization_distance_ = std::abs(d); } + void setMargin(double margin) { margin_ = margin; } + void setOverlap(double overlap) { overlap_ = overlap; } + void setToolRadius(double radius) { tool_radius_ = std::abs(radius); } + void setTraverseHeight(double height) { safe_traverse_height_ = height; } + void setVelocity(const ProcessVelocity& vel) { velocity_ = vel; } /**@brief Check if values of offset variables are acceptable */ bool variables_ok() const { - return tool_radius_ >= 0. && /*tool must be real*/ - margin_ >= 0. && /*negative margin is dangerous*/ - overlap_ < 2.*tool_radius_ && /*offset must increment inward*/ - (tool_radius_ != 0. || overlap_ != 0.) && /*offset must be positive*/ - safe_traverse_height_ >= 0.; /*negative traverse height may be inside part!*/ + return tool_radius_ >= 0. && /*tool must be real*/ + margin_ >= 0. && /*negative margin is dangerous*/ + overlap_ < 2. * tool_radius_ && /*offset must increment inward*/ + (tool_radius_ != 0. || overlap_ != 0.) && /*offset must be positive*/ + safe_traverse_height_ >= 0.; /*negative traverse height may be inside part!*/ } - bool verbose_; -private: - - //TODO comment Does not add start/end - void addInterpolatedProcessPts(const ProcessPt &start, const ProcessPt &end, double vel=0.); - //TODO comment - void addPolygonToProcessPath(const PolygonBoundary &bnd, double vel=0.); +private: + // TODO comment Does not add start/end + void addInterpolatedProcessPts(const ProcessPt& start, const ProcessPt& end, double vel = 0.); - //TODO comment - void addTraverseToProcessPath(const PolygonPt &from, const PolygonPt &to); + // TODO comment + void addPolygonToProcessPath(const PolygonBoundary& bnd, double vel = 0.); - /**@brief Create a ProcessTransition with linear velocity - * ProcessTransition will be populated with linear velocity [0, vel, double::max()] - * @param vel Desired path velocity - * @return descartes::ProcessTransition - */ - descartes::ProcessTransition velToTransition(double vel) { - descartes::ProcessTransition pt; - pt.setLinearVelocityConstraint(descartes::LinearVelocityConstraint(vel)); - return pt; - } + // TODO comment + void addTraverseToProcessPath(const PolygonPt& from, const PolygonPt& to); - double tool_radius_; /** *path_offsets_; + double + max_discretization_distance_; /**<(m) When discretizing segments, use this or less distance + between points */ - descartes::ProcessPath process_path_; - ProcessVelocity velocity_; /*** path_offsets_; + descartes::ProcessPath process_path_; + ProcessVelocity velocity_; /** OrientationConstraintPtr; @@ -63,46 +64,47 @@ typedef boost::shared_ptr OrientationConstraintPtr; class PositionConstraint { public: - PositionConstraint() {}; - virtual ~PositionConstraint() {}; + PositionConstraint(){}; + virtual ~PositionConstraint(){}; - void addBody(const bodies::BodyPtr &body) { - constraint_region_.push_back(body); } + void addBody(const bodies::BodyPtr& body) { constraint_region_.push_back(body); } - const std::vector& getConstraintRegion() { - return constraint_region_; } + const std::vector& getConstraintRegion() { return constraint_region_; } protected: - std::vector constraint_region_; + std::vector constraint_region_; }; typedef boost::shared_ptr PositionConstraintPtr; - class ProcessPt { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW public: - ProcessPt(): nominal_pose_(Eigen::Affine3d::Identity()) {}; - virtual ~ProcessPt() {}; + ProcessPt() : nominal_pose_(Eigen::Affine3d::Identity()){}; + virtual ~ProcessPt(){}; - const Eigen::Affine3d& getFrame() const {return frame_transform_;} + const Eigen::Affine3d& getFrame() const { return frame_transform_; } - Eigen::Affine3d& pose() {return nominal_pose_;} - const Eigen::Affine3d& pose() const {return nominal_pose_;} + Eigen::Affine3d& pose() { return nominal_pose_; } + const Eigen::Affine3d& pose() const { return nominal_pose_; } - void setFrame(const std::pair &frame) {pt_frame_=frame.first; frame_transform_=frame.second;} + void setFrame(const std::pair& frame) + { + pt_frame_ = frame.first; + frame_transform_ = frame.second; + } - void setPosePosition(double x, double y, double z) {nominal_pose_.translation() << x, y, z; } + void setPosePosition(double x, double y, double z) { nominal_pose_.translation() << x, y, z; } private: - Eigen::Affine3d nominal_pose_; /** min, desired, max; }; typedef boost::shared_ptr JointVelocityConstraintPtr; -struct LinearVelocityConstraint: public ProcessTransitionConstraint +struct LinearVelocityConstraint : public ProcessTransitionConstraint { - LinearVelocityConstraint(): min(0.), desired(0.), max(0.) {}; - LinearVelocityConstraint(double _min, double _desired, double _max): min(_min), desired(_desired), max(_max) {}; - LinearVelocityConstraint(double _desired): min(0.), desired(_desired), max(std::numeric_limits::max()) {}; - LinearVelocityConstraint(const LinearVelocityConstraint &other): min(other.min), desired(other.desired), max(other.max) {}; - - //TODO write - //TODO comment + LinearVelocityConstraint() : min(0.), desired(0.), max(0.){}; + LinearVelocityConstraint(double _min, double _desired, double _max) + : min(_min), desired(_desired), max(_max){}; + LinearVelocityConstraint(double _desired) + : min(0.), desired(_desired), max(std::numeric_limits::max()){}; + LinearVelocityConstraint(const LinearVelocityConstraint& other) + : min(other.min), desired(other.desired), max(other.max){}; + + // TODO write + // TODO comment virtual bool isValid(); double min, desired, max; }; typedef boost::shared_ptr LinearVelocityConstraintPtr; -struct RotationalVelocityConstraint: public ProcessTransitionConstraint +struct RotationalVelocityConstraint : public ProcessTransitionConstraint { - RotationalVelocityConstraint(): min(0.), desired(0.), max(0.) {}; - RotationalVelocityConstraint(const RotationalVelocityConstraint &other): min(other.min), desired(other.desired), max(other.max) {}; - //TODO write - //TODO comment + RotationalVelocityConstraint() : min(0.), desired(0.), max(0.){}; + RotationalVelocityConstraint(const RotationalVelocityConstraint& other) + : min(other.min), desired(other.desired), max(other.max){}; + // TODO write + // TODO comment virtual bool isValid(); double min, desired, max; }; typedef boost::shared_ptr RotationalVelocityConstraintPtr; - class ProcessTransition { public: - ProcessTransition() {}; - virtual ~ProcessTransition() {}; + ProcessTransition(){}; + virtual ~ProcessTransition(){}; - //TODO write - //TODO comment + // TODO write + // TODO comment bool constraintsValid(); - void setJointVelocityConstraint(const JointVelocityConstraint &jvc) { - joint_velocity_.reset(new JointVelocityConstraint(jvc)); } - void setJointVelocityConstraint(const JointVelocityConstraintPtr &jvc) { - joint_velocity_ = jvc; } - - void setLinearVelocityConstraint(const LinearVelocityConstraint &lvc) { - linear_velocity_.reset(new LinearVelocityConstraint(lvc)); } - void setLinearVelocityConstraint(const LinearVelocityConstraintPtr &lvc) { - linear_velocity_ = lvc; } - - void setRotationalVelocityConstraint(const RotationalVelocityConstraint &rvc) { - rotational_velocity_.reset(new RotationalVelocityConstraint(rvc)); } - void setRotationalVelocityConstraint(const RotationalVelocityConstraintPtr &rvc) { - rotational_velocity_ = rvc; } - - const LinearVelocityConstraintPtr& getLinearVelocity() const {return linear_velocity_;} + void setJointVelocityConstraint(const JointVelocityConstraint& jvc) + { + joint_velocity_.reset(new JointVelocityConstraint(jvc)); + } + void setJointVelocityConstraint(const JointVelocityConstraintPtr& jvc) { joint_velocity_ = jvc; } + + void setLinearVelocityConstraint(const LinearVelocityConstraint& lvc) + { + linear_velocity_.reset(new LinearVelocityConstraint(lvc)); + } + void setLinearVelocityConstraint(const LinearVelocityConstraintPtr& lvc) + { + linear_velocity_ = lvc; + } + + void setRotationalVelocityConstraint(const RotationalVelocityConstraint& rvc) + { + rotational_velocity_.reset(new RotationalVelocityConstraint(rvc)); + } + void setRotationalVelocityConstraint(const RotationalVelocityConstraintPtr& rvc) + { + rotational_velocity_ = rvc; + } + + const LinearVelocityConstraintPtr& getLinearVelocity() const { return linear_velocity_; } protected: - JointVelocityConstraintPtr joint_velocity_; /** -inline std::vector discretizeArc2D(const Pt &p1, const Pt &p2, const Pt &c, bool positive_rotation, double max_sep) +inline std::vector discretizeArc2D(const Pt& p1, const Pt& p2, const Pt& c, + bool positive_rotation, double max_sep) { std::vector vec; - vec.push_back(p1); // Add first point + vec.push_back(p1); // Add first point // Find angle from p1 to p2. Check direction of theta against known direction of arc. - double r = c.dist(p2); // TODO check for equal radius from c-p1? - double theta = std::atan2( (p1-c).cross(p2-c), (p1-c).dot(p2-c) ); - if (theta<0. && positive_rotation) + double r = c.dist(p2); // TODO check for equal radius from c-p1? + double theta = std::atan2((p1 - c).cross(p2 - c), (p1 - c).dot(p2 - c)); + if (theta < 0. && positive_rotation) { - theta += 2.*M_PI; + theta += 2. * M_PI; } - else if (theta>0. && !positive_rotation) + else if (theta > 0. && !positive_rotation) { - theta -= 2.*M_PI; + theta -= 2. * M_PI; } - double max_theta(max_sep/r); + double max_theta(max_sep / r); if (std::abs(theta) > max_theta) { // Perform rotation by transform matrix: rotate p1 by theta about central axis - size_t new_ptcnt = static_cast(std::ceil(std::abs(theta/max_theta)) -1.); - double disc_angle = theta/static_cast(new_ptcnt+1.); - Eigen::Vector3d start_pt(p1.x-c.x, p1.y-c.y, 1.); // Start point expressed in local frame (c in global frame) - for (size_t ii=1; ii<=new_ptcnt; ++ii) + size_t new_ptcnt = static_cast(std::ceil(std::abs(theta / max_theta)) - 1.); + double disc_angle = theta / static_cast(new_ptcnt + 1.); + Eigen::Vector3d start_pt(p1.x - c.x, p1.y - c.y, + 1.); // Start point expressed in local frame (c in global frame) + for (size_t ii = 1; ii <= new_ptcnt; ++ii) { - double angle = disc_angle * static_cast(ii); // what angle this point is added at + double angle = disc_angle * static_cast(ii); // what angle this point is added at Eigen::Matrix3d rotator; rotator << cos(angle), -sin(angle), c.x, sin(angle), cos(angle), c.y, 0, 0, 1; - Eigen::Vector3d new_pt = rotator * start_pt; // New point in global frame + Eigen::Vector3d new_pt = rotator * start_pt; // New point in global frame vec.push_back(Pt(new_pt(0), new_pt(1))); } } @@ -88,41 +88,44 @@ inline std::vector discretizeArc2D(const Pt &p1, const Pt &p2, const Pt &c, } template -inline std::vector discretizeLinear(const Pt &p1, const Pt &p2, double max_sep) +inline std::vector discretizeLinear(const Pt& p1, const Pt& p2, double max_sep) { - //add [start-end) with interpolated points between + // add [start-end) with interpolated points between std::vector vec; - vec.push_back(p1); // Add first point + vec.push_back(p1); // Add first point double sep = p1.dist(p2); if (sep > max_sep) { - size_t new_ptcnt = static_cast(std::ceil(sep/max_sep) - 1.); - double disc_dist = sep/static_cast(new_ptcnt+1.); - for (size_t ii=1; ii<=new_ptcnt; ++ii) + size_t new_ptcnt = static_cast(std::ceil(sep / max_sep) - 1.); + double disc_dist = sep / static_cast(new_ptcnt + 1.); + for (size_t ii = 1; ii <= new_ptcnt; ++ii) { - vec.push_back(p1 + (p2-p1)*(static_cast(ii)*disc_dist/sep)); + vec.push_back(p1 + (p2 - p1) * (static_cast(ii) * disc_dist / sep)); } } return vec; } -} /* namespace geometry */ +} /* namespace geometry */ namespace translations { -/**@brief Convert a godel type to a geometry_msg type. This function operates on PolygonBoundaryCollection and vector. - * @param polygons_msg vector of Polygons populated from PolygonBoundaryCollection. Z-value is unchanged. +/**@brief Convert a godel type to a geometry_msg type. This function operates on + * PolygonBoundaryCollection and vector. + * @param polygons_msg vector of Polygons populated from PolygonBoundaryCollection. Z-value is + * unchanged. * @param pbc Collection of PolygonBoundaries. */ -inline void godelToGeometryMsgs(std::vector &polygons_msg, const godel_process_path::PolygonBoundaryCollection &pbc) +inline void godelToGeometryMsgs(std::vector& polygons_msg, + const godel_process_path::PolygonBoundaryCollection& pbc) { polygons_msg.clear(); - BOOST_FOREACH(::godel_process_path::PolygonBoundary polygon, pbc) + BOOST_FOREACH (::godel_process_path::PolygonBoundary polygon, pbc) { geometry_msgs::Polygon polygon_msg; - BOOST_FOREACH(godel_process_path::PolygonPt pt, polygon) + BOOST_FOREACH (godel_process_path::PolygonPt pt, polygon) { geometry_msgs::Point32 pt_msg; pt_msg.x = pt.x; @@ -133,17 +136,20 @@ inline void godelToGeometryMsgs(std::vector &polygons_ms } } -/**@brief Convert a geometry_msg type to a godel type. This function operates on vector and PolygonBoundaryCollection. +/**@brief Convert a geometry_msg type to a godel type. This function operates on vector and + * PolygonBoundaryCollection. * @param pbc Collection of PolygonBoundaries. - * @param polygons_msg vector of Polygons populated from PolygonBoundaryCollection. Z-value is ignored. + * @param polygons_msg vector of Polygons populated from PolygonBoundaryCollection. Z-value is + * ignored. */ -inline void geometryMsgsToGodel(godel_process_path::PolygonBoundaryCollection &pbc, const std::vector &polygons_msg) +inline void geometryMsgsToGodel(godel_process_path::PolygonBoundaryCollection& pbc, + const std::vector& polygons_msg) { pbc.clear(); - BOOST_FOREACH(geometry_msgs::Polygon polygon_msg, polygons_msg) + BOOST_FOREACH (geometry_msgs::Polygon polygon_msg, polygons_msg) { godel_process_path::PolygonBoundary polygon; - BOOST_FOREACH(geometry_msgs::Point32 pt, polygon_msg.points) + BOOST_FOREACH (geometry_msgs::Point32 pt, polygon_msg.points) { polygon.push_back(godel_process_path::PolygonPt(pt.x, pt.y)); } @@ -151,8 +157,8 @@ inline void geometryMsgsToGodel(godel_process_path::PolygonBoundaryCollection &p } } - -/**@brief Convert a godel type to a visualization_msg type. This function operates on PolygonBoundary and Marker. +/**@brief Convert a godel type to a visualization_msg type. This function operates on + * PolygonBoundary and Marker. * Creates a line list with default color and 1mm thickness. * User's responsibility to complete remainder of message. * @param marker Marker msg populated with points @@ -160,11 +166,13 @@ inline void geometryMsgsToGodel(godel_process_path::PolygonBoundaryCollection &p * @param default_color Color to use for all points in marker. * @param default_scale Line width. */ -inline void godelToVisualizationMsgs(visualization_msgs::Marker &marker, const godel_process_path::PolygonBoundary &polygon, - std_msgs::ColorRGBA default_color = std_msgs::ColorRGBA(), double default_scale = .001) +inline void godelToVisualizationMsgs(visualization_msgs::Marker& marker, + const godel_process_path::PolygonBoundary& polygon, + std_msgs::ColorRGBA default_color = std_msgs::ColorRGBA(), + double default_scale = .001) { marker.type = visualization_msgs::Marker::LINE_STRIP; - BOOST_FOREACH(::godel_process_path::PolygonPt pt, polygon) + BOOST_FOREACH (::godel_process_path::PolygonPt pt, polygon) { geometry_msgs::Point pt_msg; pt_msg.x = pt.x; @@ -175,7 +183,8 @@ inline void godelToVisualizationMsgs(visualization_msgs::Marker &marker, const g marker.scale.x = default_scale; } -/**@brief Convert a godel type to a visualization_msg type. This function operates on PolygonBoundaryCollection and MarkerArray. +/**@brief Convert a godel type to a visualization_msg type. This function operates on + * PolygonBoundaryCollection and MarkerArray. * Creates a series of line lists with default color and 1mm thickness. * User's responsibility to complete remainder of message. * @param marker MarkerArray msg populated with line list markers. One marker per PolygonBoundary @@ -183,11 +192,13 @@ inline void godelToVisualizationMsgs(visualization_msgs::Marker &marker, const g * @param default_color Color to use for all points in all markers. * @param default_scale Line width for all markers. */ -inline void godelToVisualizationMsgs(visualization_msgs::MarkerArray &markers, const godel_process_path::PolygonBoundaryCollection &pbc, - std_msgs::ColorRGBA default_color = std_msgs::ColorRGBA(), double default_scale = .001) +inline void godelToVisualizationMsgs(visualization_msgs::MarkerArray& markers, + const godel_process_path::PolygonBoundaryCollection& pbc, + std_msgs::ColorRGBA default_color = std_msgs::ColorRGBA(), + double default_scale = .001) { markers.markers.clear(); - BOOST_FOREACH(::godel_process_path::PolygonBoundary polygon, pbc) + BOOST_FOREACH (::godel_process_path::PolygonBoundary polygon, pbc) { visualization_msgs::Marker marker; godelToVisualizationMsgs(marker, polygon, default_color, default_scale); @@ -195,7 +206,7 @@ inline void godelToVisualizationMsgs(visualization_msgs::MarkerArray &markers, c } } -} /* namespace translations */ +} /* namespace translations */ /* General utility functions */ @@ -203,7 +214,7 @@ inline void godelToVisualizationMsgs(visualization_msgs::MarkerArray &markers, c * @return True if item exists in container. */ template -inline bool exists(const Item &item, const Container &container) +inline bool exists(const Item& item, const Container& container) { return std::find(container.begin(), container.end(), item) != container.end(); } @@ -215,9 +226,9 @@ inline bool exists(const Item &item, const Container &container) * @return True if child found. If return is false, child left unchanged. */ template -inline bool getFirstChild(typename ::boost::graph_traits::vertex_descriptor &child, - const typename ::boost::graph_traits::vertex_descriptor &parent, - const Graph &g) +inline bool getFirstChild(typename ::boost::graph_traits::vertex_descriptor& child, + const typename ::boost::graph_traits::vertex_descriptor& parent, + const Graph& g) { typename boost::graph_traits::out_edge_iterator edge, edges_end; boost::tie(edge, edges_end) = boost::out_edges(parent, g); @@ -232,12 +243,13 @@ inline bool getFirstChild(typename ::boost::graph_traits::vertex_descript /**@brief Move an item from from container to another. * Designed to work with STL iterators * @param from Container containing iterator. Iter is removed from container - * @param to Container to move to. If successfully completed, item referenced by "iter" is added to "to". + * @param to Container to move to. If successfully completed, item referenced by "iter" is added to + * "to". * @param iter Iterator into "from" container * @return True if item moved successfully, false if "from" does not contain "iter" */ template -inline bool moveIterFrom(Container &from, Container &to, typename Container::iterator iter) +inline bool moveIterFrom(Container& from, Container& to, typename Container::iterator iter) { if (iter >= from.begin() && iter < from.end()) { @@ -256,10 +268,10 @@ inline bool moveIterFrom(Container &from, Container &to, typename Container::ite * @return True if item moved successfully, false if "from" does not contain "item" */ template -inline bool moveItemFrom(Container &from, Container &to, const Item &item) +inline bool moveItemFrom(Container& from, Container& to, const Item& item) { typename Container::iterator iter = std::find(from.begin(), from.end(), item); - if (iter!=from.end()) + if (iter != from.end()) { moveIterFrom(from, to, iter); return true; @@ -267,7 +279,6 @@ inline bool moveItemFrom(Container &from, Container &to, const Item &item) return false; } - -} /* end utils */ -} /* end godel_process_path */ +} /* end utils */ +} /* end godel_process_path */ #endif /* UTILS_H_ */ diff --git a/godel_process_path_generation/src/mesh_importer.cpp b/godel_process_path_generation/src/mesh_importer.cpp index db7b254d..72189317 100644 --- a/godel_process_path_generation/src/mesh_importer.cpp +++ b/godel_process_path_generation/src/mesh_importer.cpp @@ -44,128 +44,129 @@ namespace godel_process_path const double CONCAVE_HULL_SIDE_LENGHT = 0.01f; -bool MeshImporter::applyConcaveHull(const Cloud& in,pcl::ModelCoefficients &plane_coeffs,geometry_msgs::PolygonStamped& polygon) +bool MeshImporter::applyConcaveHull(const Cloud& in, pcl::ModelCoefficients& plane_coeffs, + geometry_msgs::PolygonStamped& polygon) { - typedef pcl::PointCloud Cld; - - Cld plane_cloud; - pcl::copyPointCloud(in,plane_cloud); - - - // Project the model inliers - Cld::Ptr cloud_projected_ptr(new Cld()); - pcl::ModelCoefficients::Ptr coeffs_ptr(new pcl::ModelCoefficients(plane_coeffs)); - - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (plane_cloud.makeShared()); - proj.setModelCoefficients (coeffs_ptr); - proj.filter (*cloud_projected_ptr); - - // Create a Concave Hull representation of the projected inliers - pcl::PointCloud::Ptr cloud_hull (new pcl::PointCloud); - //pcl::ConcaveHull chull; - pcl::ConvexHull chull; - chull.setInputCloud (cloud_projected_ptr); - //chull.setAlpha (CONCAVE_HULL_SIDE_LENGHT); - chull.setDimension(2); - chull.reconstruct (*cloud_hull); - - if(cloud_hull->empty()) - { - ROS_ERROR_STREAM("Concave hull failed to bound region"); - return false; - } - - // creating polygon - geometry_msgs::Point32 p2; - for(int i = 0 ; i < cloud_hull->points.size();i++) - { - - p2.x = cloud_hull->points[i].x; - p2.y = cloud_hull->points[i].y; - p2.z = cloud_hull->points[i].z; - - //polygon.polygon.points.push_back(p1); - polygon.polygon.points.push_back(p2); - } - - return true; + typedef pcl::PointCloud Cld; + + Cld plane_cloud; + pcl::copyPointCloud(in, plane_cloud); + + // Project the model inliers + Cld::Ptr cloud_projected_ptr(new Cld()); + pcl::ModelCoefficients::Ptr coeffs_ptr(new pcl::ModelCoefficients(plane_coeffs)); + + pcl::ProjectInliers proj; + proj.setModelType(pcl::SACMODEL_PLANE); + proj.setInputCloud(plane_cloud.makeShared()); + proj.setModelCoefficients(coeffs_ptr); + proj.filter(*cloud_projected_ptr); + + // Create a Concave Hull representation of the projected inliers + pcl::PointCloud::Ptr cloud_hull(new pcl::PointCloud); + // pcl::ConcaveHull chull; + pcl::ConvexHull chull; + chull.setInputCloud(cloud_projected_ptr); + // chull.setAlpha (CONCAVE_HULL_SIDE_LENGHT); + chull.setDimension(2); + chull.reconstruct(*cloud_hull); + + if (cloud_hull->empty()) + { + ROS_ERROR_STREAM("Concave hull failed to bound region"); + return false; + } + + // creating polygon + geometry_msgs::Point32 p2; + for (int i = 0; i < cloud_hull->points.size(); i++) + { + + p2.x = cloud_hull->points[i].x; + p2.y = cloud_hull->points[i].y; + p2.z = cloud_hull->points[i].z; + + // polygon.polygon.points.push_back(p1); + polygon.polygon.points.push_back(p2); + } + + return true; } -bool MeshImporter::calculateSimpleBoundary(const pcl::PolygonMesh &input_mesh) +bool MeshImporter::calculateSimpleBoundary(const pcl::PolygonMesh& input_mesh) { - typedef pcl::geometry::TriangleMesh > Mesh; - - plane_frame_.setIdentity(); - boundaries_.clear(); - - Cloud::Ptr points(new Cloud); - pcl::fromPCLPointCloud2(input_mesh.cloud,*points); - - - /* Find plane coefficients from point cloud data. - * Find centroid of point cloud as origin of local coordinate system. - * Create local coordinate frame for plane. - */ - Eigen::Hyperplane hplane; - if (!computePlaneCoefficients(points, hplane.coeffs())) - { - ROS_WARN("Could not compute plane coefficients"); - return false; - } - if (verbose_) - { - ROS_INFO_STREAM("Normal: " << hplane.coeffs().transpose()); - } - - // computes local frame and saves it into the 'plane_frame_' member - Eigen::Vector4d centroid4; - pcl::compute3DCentroid(*points, centroid4); - computeLocalPlaneFrame(hplane, centroid4, *points); - if (verbose_) - { - ROS_INFO_STREAM("Local plane calculated with normal " << hplane.coeffs().transpose() << " and origin " << centroid4.transpose()); - } - - // applying concave hull - pcl::ModelCoefficients coeffs; - geometry_msgs::PolygonStamped polygon; - coeffs.values.clear(); - coeffs.values.push_back(hplane.coeffs()(0)); - coeffs.values.push_back(hplane.coeffs()(1)); - coeffs.values.push_back(hplane.coeffs()(2)); - coeffs.values.push_back(hplane.coeffs()(3)); - applyConcaveHull(*points,coeffs,polygon); - - PolygonBoundary pbound; - Eigen::Affine3d plane_inverse = plane_frame_.inverse(); - for(int i = 0; i < polygon.polygon.points.size(); i++) - { - geometry_msgs::Point32 &p = polygon.polygon.points[i]; - - Eigen::Vector3d projected_pt = hplane.projection(Eigen::Vector3d(p.x, p.y, p.z)); - Eigen::Vector3d plane_pt = plane_inverse * projected_pt; - - - // Check that plane/transform calculations are accurate by testing that transformed points lie on local plane - if (std::abs(plane_pt(2)) > .001) - { - ROS_ERROR_STREAM("z-value of projected/transformed point should be (near) 0 [" << plane_pt.transpose() << "]"); - ROS_ERROR_STREAM("Transform matrix used to project points:\n" << plane_frame_.matrix()); - return false; - } - pbound.push_back(PolygonPt(plane_pt(0), plane_pt(1))); - } - - boundaries_.push_back(pbound); - - ROS_INFO_STREAM("Added 1 boundary with "< > Mesh; + + plane_frame_.setIdentity(); + boundaries_.clear(); + + Cloud::Ptr points(new Cloud); + pcl::fromPCLPointCloud2(input_mesh.cloud, *points); + + /* Find plane coefficients from point cloud data. + * Find centroid of point cloud as origin of local coordinate system. + * Create local coordinate frame for plane. + */ + Eigen::Hyperplane hplane; + if (!computePlaneCoefficients(points, hplane.coeffs())) + { + ROS_WARN("Could not compute plane coefficients"); + return false; + } + if (verbose_) + { + ROS_INFO_STREAM("Normal: " << hplane.coeffs().transpose()); + } + + // computes local frame and saves it into the 'plane_frame_' member + Eigen::Vector4d centroid4; + pcl::compute3DCentroid(*points, centroid4); + computeLocalPlaneFrame(hplane, centroid4, *points); + if (verbose_) + { + ROS_INFO_STREAM("Local plane calculated with normal " + << hplane.coeffs().transpose() << " and origin " << centroid4.transpose()); + } + + // applying concave hull + pcl::ModelCoefficients coeffs; + geometry_msgs::PolygonStamped polygon; + coeffs.values.clear(); + coeffs.values.push_back(hplane.coeffs()(0)); + coeffs.values.push_back(hplane.coeffs()(1)); + coeffs.values.push_back(hplane.coeffs()(2)); + coeffs.values.push_back(hplane.coeffs()(3)); + applyConcaveHull(*points, coeffs, polygon); + + PolygonBoundary pbound; + Eigen::Affine3d plane_inverse = plane_frame_.inverse(); + for (int i = 0; i < polygon.polygon.points.size(); i++) + { + geometry_msgs::Point32& p = polygon.polygon.points[i]; + + Eigen::Vector3d projected_pt = hplane.projection(Eigen::Vector3d(p.x, p.y, p.z)); + Eigen::Vector3d plane_pt = plane_inverse * projected_pt; + + // Check that plane/transform calculations are accurate by testing that transformed points lie + // on local plane + if (std::abs(plane_pt(2)) > .001) + { + ROS_ERROR_STREAM("z-value of projected/transformed point should be (near) 0 [" + << plane_pt.transpose() << "]"); + ROS_ERROR_STREAM("Transform matrix used to project points:\n" << plane_frame_.matrix()); + return false; + } + pbound.push_back(PolygonPt(plane_pt(0), plane_pt(1))); + } + + boundaries_.push_back(pbound); + + ROS_INFO_STREAM("Added 1 boundary with " << boundaries_[0].size() << " points"); + + return true; } -bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh &input_mesh) +bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh& input_mesh) { typedef pcl::geometry::TriangleMesh > Mesh; @@ -173,8 +174,7 @@ bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh &input_mesh) boundaries_.clear(); Cloud::Ptr points(new Cloud); - pcl::fromPCLPointCloud2(input_mesh.cloud,*points); - + pcl::fromPCLPointCloud2(input_mesh.cloud, *points); /* Find plane coefficients from point cloud data. * Find centroid of point cloud as origin of local coordinate system. @@ -196,10 +196,10 @@ bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh &input_mesh) computeLocalPlaneFrame(hplane, centroid4, *points); if (verbose_) { - ROS_INFO_STREAM("Local plane calculated with normal " << hplane.coeffs().transpose() << " and origin " << centroid4.transpose()); + ROS_INFO_STREAM("Local plane calculated with normal " + << hplane.coeffs().transpose() << " and origin " << centroid4.transpose()); } - /* Use pcl::geometry::TriangleMesh to extract boundaries. * Project boundaries to local plane, and add to boundaries_ list. * Note: External boundary is CCW ordered, internal boundaries are CW ordered. @@ -209,16 +209,18 @@ bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh &input_mesh) MeshIndexMap mesh_index_map; for (size_t ii = 0; ii < input_mesh.polygons.size(); ++ii) { - const std::vector &vertices = input_mesh.polygons.at(ii).vertices; + const std::vector& vertices = input_mesh.polygons.at(ii).vertices; if (vertices.size() != 3) { - ROS_ERROR_STREAM("Found polygon with " << vertices.size() << " sides, only triangle mesh supported!"); + ROS_ERROR_STREAM("Found polygon with " << vertices.size() + << " sides, only triangle mesh supported!"); return false; } Mesh::VertexIndices vi; - for (std::vector::const_iterator vidx = vertices.begin(), viend = vertices.end(); vidx != viend; ++vidx) + for (std::vector::const_iterator vidx = vertices.begin(), viend = vertices.end(); + vidx != viend; ++vidx) { -// mesh_index_map2.left.count + // mesh_index_map2.left.count if (!mesh_index_map.left.count(*vidx)) { mesh_index_map.insert(MeshIndexMap::value_type(*vidx, mesh.addVertex())); @@ -233,22 +235,27 @@ bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh &input_mesh) pcl_godel::geometry::getBoundBoundaryHalfEdges(mesh, boundary_he_indices); // For each boundary, project boundary points onto plane and add to boundaries_ - Eigen::Affine3d plane_inverse = plane_frame_.inverse(); // Pre-compute inverse - for (std::vector::const_iterator boundary = boundary_he_indices.begin(), b_end = boundary_he_indices.end(); + Eigen::Affine3d plane_inverse = plane_frame_.inverse(); // Pre-compute inverse + for (std::vector::const_iterator boundary = boundary_he_indices.begin(), + b_end = boundary_he_indices.end(); boundary != b_end; ++boundary) { PolygonBoundary pbound; for (Mesh::HalfEdgeIndices::const_iterator edge = boundary->begin(), edge_end = boundary->end(); edge != edge_end; ++edge) { - Cloud::PointType cloudpt = points->points.at(mesh_index_map.right.at(mesh.getOriginatingVertexIndex(*edge))); // pt on boundary - Eigen::Vector3d projected_pt = hplane.projection(Eigen::Vector3d(cloudpt.x, cloudpt.y, cloudpt.z)); // pt projected onto plane - Eigen::Vector3d plane_pt = plane_inverse * projected_pt; // pt in plane frame - - // Check that plane/transform calculations are accurate by testing that transformed points lie on local plane + Cloud::PointType cloudpt = points->points.at( + mesh_index_map.right.at(mesh.getOriginatingVertexIndex(*edge))); // pt on boundary + Eigen::Vector3d projected_pt = hplane.projection( + Eigen::Vector3d(cloudpt.x, cloudpt.y, cloudpt.z)); // pt projected onto plane + Eigen::Vector3d plane_pt = plane_inverse * projected_pt; // pt in plane frame + + // Check that plane/transform calculations are accurate by testing that transformed points lie + // on local plane if (std::abs(plane_pt(2)) > .001) { - ROS_ERROR_STREAM("z-value of projected/transformed point should be (near) 0 [" << plane_pt.transpose() << "]"); + ROS_ERROR_STREAM("z-value of projected/transformed point should be (near) 0 [" + << plane_pt.transpose() << "]"); ROS_ERROR_STREAM("Transform matrix used to project points:\n" << plane_frame_.matrix()); return false; } @@ -261,22 +268,23 @@ bool MeshImporter::calculateBoundaryData(const pcl::PolygonMesh &input_mesh) return true; } -void MeshImporter::computeLocalPlaneFrame(const Eigen::Hyperplane &plane, const Vector4d ¢roid, const Cloud& cloud) +void MeshImporter::computeLocalPlaneFrame(const Eigen::Hyperplane& plane, + const Vector4d& centroid, const Cloud& cloud) { - Eigen::Vector3d origin = plane.projection(centroid.head<3>()); // Project centroid onto plane + Eigen::Vector3d origin = plane.projection(centroid.head<3>()); // Project centroid onto plane const Eigen::Vector3d& plane_normal = plane.coeffs().head<3>(); // Compute major axis of the part Eigen::Matrix3d covar_matrix; - pcl::computeCovarianceMatrixNormalized (cloud, centroid, covar_matrix); + pcl::computeCovarianceMatrixNormalized(cloud, centroid, covar_matrix); // Find eigenvectors - Eigen::Matrix3d evecs; // eigenvectors + Eigen::Matrix3d evecs; // eigenvectors Eigen::Vector3d evals; // eigenvalues - pcl::eigen33 (covar_matrix, evecs, evals); + pcl::eigen33(covar_matrix, evecs, evals); // Y-axis can be estimated from the 2nd eigenvector - Eigen::Vector3d y_axis (evecs (0, 1), evecs (1, 1), evecs (2, 1)); + Eigen::Vector3d y_axis(evecs(0, 1), evecs(1, 1), evecs(2, 1)); y_axis = y_axis.normalized(); // Z-axis computed in plane segmentation step - Eigen::Vector3d z_axis (plane_normal.normalized()); + Eigen::Vector3d z_axis(plane_normal.normalized()); // Cross product to obtain X axis Eigen::Vector3d x_axis = y_axis.cross(z_axis); @@ -287,25 +295,25 @@ void MeshImporter::computeLocalPlaneFrame(const Eigen::Hyperplane &pl plane_frame_.translation() = origin; } -bool MeshImporter::computePlaneCoefficients(Cloud::ConstPtr cloud, Eigen::Vector4d &output) +bool MeshImporter::computePlaneCoefficients(Cloud::ConstPtr cloud, Eigen::Vector4d& output) { pcl::ModelCoefficients coefficients; - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation seg; // Optional - seg.setOptimizeCoefficients (true); + seg.setOptimizeCoefficients(true); // Mandatory - seg.setModelType (pcl::SACMODEL_PLANE); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setDistanceThreshold (0.01); - const Cloud::PointType &p0 = cloud->points.at(0); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(0.01); + const Cloud::PointType& p0 = cloud->points.at(0); Eigen::Vector3f expected_normal(p0.normal_x, p0.normal_y, p0.normal_z); seg.setAxis(expected_normal); seg.setEpsAngle(0.5); - seg.setInputCloud (cloud); - seg.segment (*inliers, coefficients); + seg.setInputCloud(cloud); + seg.segment(*inliers, coefficients); if (coefficients.values.size() != 4) { ROS_ERROR("Expect 4 coefficients for plane, got %li", coefficients.values.size()); @@ -314,16 +322,20 @@ bool MeshImporter::computePlaneCoefficients(Cloud::ConstPtr cloud, Eigen::Vector // Check that points match a plane fit double fraction_inclusive = 0.9; - if (static_cast(inliers->indices.size())/static_cast(cloud->height * cloud->width) < fraction_inclusive) + if (static_cast(inliers->indices.size()) / + static_cast(cloud->height * cloud->width) < + fraction_inclusive) { ROS_WARN("Less than 90%% of points included in plane fit."); return false; } ROS_INFO_COND(verbose_, "%f percent of points included in plane fit.", - static_cast(inliers->indices.size())/static_cast(cloud->height * cloud->width)*100.); + static_cast(inliers->indices.size()) / + static_cast(cloud->height * cloud->width) * 100.); // Check that normal is aligned with pointcloud data - Eigen::Vector3f actual_normal(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2)); + Eigen::Vector3f actual_normal(coefficients.values.at(0), coefficients.values.at(1), + coefficients.values.at(2)); if (actual_normal.dot(expected_normal) < 0.0) { ROS_WARN("Flipping RANSAC plane normal"); @@ -338,4 +350,3 @@ bool MeshImporter::computePlaneCoefficients(Cloud::ConstPtr cloud, Eigen::Vector } } /* namespace godel_process_path */ - diff --git a/godel_process_path_generation/src/polygon_utils.cpp b/godel_process_path_generation/src/polygon_utils.cpp index e7d9c2c7..e96094a3 100644 --- a/godel_process_path_generation/src/polygon_utils.cpp +++ b/godel_process_path_generation/src/polygon_utils.cpp @@ -32,7 +32,7 @@ namespace godel_process_path namespace polygon_utils { -bool PolygonSegment::intersects(const PolygonSegment &other, double tol) const +bool PolygonSegment::intersects(const PolygonSegment& other, double tol) const { /* Two vectors: intersection pt = p1 + u(p2-p1) = p3 + v(p4-p3) * u(p2-p1) + v(-p4+p3) = (p3-p1) = del @@ -42,20 +42,20 @@ bool PolygonSegment::intersects(const PolygonSegment &other, double tol) const double denom = -cross(other); if (std::abs(denom) < 1e-12) { - return false; // Lines are (nearly) parallel + return false; // Lines are (nearly) parallel } // Solve for u PolygonPt del = other.start - this->start; double u = -del.cross(other.vector()) / denom; - if (u < 0.-tol || u > 1.+tol) + if (u < 0. - tol || u > 1. + tol) { // Intersection is outside of segment 1 return false; } else { // Possible intersection, must check segment 2 double v = this->vector().cross(del) / denom; - if (v < 0.-tol || v > 1.+tol) + if (v < 0. - tol || v > 1. + tol) { return false; } @@ -63,11 +63,11 @@ bool PolygonSegment::intersects(const PolygonSegment &other, double tol) const return true; } -void boundaryToSegments(std::vector &segments, const PolygonBoundary &polygon) +void boundaryToSegments(std::vector& segments, const PolygonBoundary& polygon) { segments.clear(); PolygonBoundary::const_iterator pt, last_pt; - for (pt=polygon.begin(), last_pt=boost::prior(polygon.end()); pt!=polygon.end(); ++pt) + for (pt = polygon.begin(), last_pt = boost::prior(polygon.end()); pt != polygon.end(); ++pt) { if (pt == last_pt) { @@ -80,13 +80,14 @@ void boundaryToSegments(std::vector &segments, const PolygonBoun } } -void filter(PolygonBoundary &boundary, double tol) +void filter(PolygonBoundary& boundary, double tol) { - // Check bounds of tol. Tol is input as an angle measurement, but used internally as the sine of that angle. + // Check bounds of tol. Tol is input as an angle measurement, but used internally as the sine of + // that angle. if (tol > M_PI_2) { ROS_WARN("Global filter tolerance set above pi/2 (%lf), using %f", tol, M_PI_2); - tol = 1.; // M_PI_2 + tol = 1.; // M_PI_2 } else if (tol < 0.) { @@ -107,19 +108,19 @@ void filter(PolygonBoundary &boundary, double tol) PolygonSegment start_seg(*start, *boost::next(start)); while (ros::ok()) { - if (boost::next(start,2) == boundary.end()) + if (boost::next(start, 2) == boundary.end()) { return; } - PolygonSegment next_seg(*boost::next(start), *boost::next(start,2)); + PolygonSegment next_seg(*boost::next(start), *boost::next(start, 2)); double kross = start_seg.cross(next_seg); - if (kross*kross/start_seg.length2()/next_seg.length2() < tol) - { /* found linear segment */ + if (kross * kross / start_seg.length2() / next_seg.length2() < tol) + { /* found linear segment */ boundary.erase(boost::next(start)); start_seg.end = *boost::next(start); } else - { /* move to next segment */ + { /* move to next segment */ ++start; start_seg.start = *start; start_seg.end = *boost::next(start); @@ -127,7 +128,7 @@ void filter(PolygonBoundary &boundary, double tol) } } -bool intersects(const PolygonBoundary &a, const PolygonBoundary &b) +bool intersects(const PolygonBoundary& a, const PolygonBoundary& b) { std::vector v_a, v_b; boundaryToSegments(v_a, a); @@ -135,11 +136,11 @@ bool intersects(const PolygonBoundary &a, const PolygonBoundary &b) return intersects(v_a, v_b); } -bool intersects(const std::vector &a, const std::vector &b) +bool intersects(const std::vector& a, const std::vector& b) { - BOOST_FOREACH(const PolygonSegment &seg_a, a) + BOOST_FOREACH (const PolygonSegment& seg_a, a) { - BOOST_FOREACH(const PolygonSegment &seg_b, b) + BOOST_FOREACH (const PolygonSegment& seg_b, b) { if (seg_a.intersects(seg_b)) { @@ -150,7 +151,7 @@ bool intersects(const std::vector &a, const std::vector::epsilon(); + const double LENGTH_TOL = 100. * std::numeric_limits::epsilon(); PolygonBoundary::const_iterator pt, last_pt; int pt_counter = 0; - for (pt=bnd.begin(), last_pt=boost::prior(bnd.end()); pt!=bnd.end(); ++pt) + for (pt = bnd.begin(), last_pt = boost::prior(bnd.end()); pt != bnd.end(); ++pt) { - if (pt==last_pt) + if (pt == last_pt) { if (pt->dist(bnd.front()) < LENGTH_TOL) { - ROS_WARN_STREAM("Invisible polygon boundary segment at point "<dist(*boost::next(pt)) < LENGTH_TOL) { - ROS_WARN_STREAM("Invisible polygon boundary segment at point "< segments; boundaryToSegments(segments, bnd); - for (std::vector::const_iterator seg=segments.begin(); seg!=(segments.end()-2); ++seg) + for (std::vector::const_iterator seg = segments.begin(); + seg != (segments.end() - 2); ++seg) { - for (std::vector::const_iterator other_seg=boost::next(seg,2); other_seg!=segments.end(); ++other_seg) + for (std::vector::const_iterator other_seg = boost::next(seg, 2); + other_seg != segments.end(); ++other_seg) { - if (seg==segments.begin() && other_seg==boost::prior(segments.end())) + if (seg == segments.begin() && other_seg == boost::prior(segments.end())) { continue; } if (seg->intersects(*other_seg)) { - ROS_WARN_STREAM("Self-intersecting polygon at segments " << seg-segments.begin() << " - " << other_seg-segments.begin() << " / " << segments.size()); + ROS_WARN_STREAM("Self-intersecting polygon at segments " + << seg - segments.begin() << " - " << other_seg - segments.begin() << " / " + << segments.size()); return false; } } @@ -210,10 +215,10 @@ bool checkBoundary(const PolygonBoundary &bnd) return true; } -bool checkBoundaryCollection(const PolygonBoundaryCollection &pbc) +bool checkBoundaryCollection(const PolygonBoundaryCollection& pbc) { // Check each boundary individually - BOOST_FOREACH(const PolygonBoundary &bnd, pbc) + BOOST_FOREACH (const PolygonBoundary& bnd, pbc) { if (!checkBoundary(bnd)) /*call check boundary without intersection checks*/ { @@ -224,31 +229,31 @@ bool checkBoundaryCollection(const PolygonBoundaryCollection &pbc) // Precompute vectors of segments for polygonboundaries typedef std::vector > SegmentsList; SegmentsList segments_list; - BOOST_FOREACH(const PolygonBoundary &bnd, pbc) + BOOST_FOREACH (const PolygonBoundary& bnd, pbc) { std::vector segments; boundaryToSegments(segments, bnd); segments_list.push_back(segments); } -// // Check global intersections between boundaries -// BOOST_FOREACH(const PolygonBoundary &a, pbc) -// { -// BOOST_FOREACH(const PolygonBoundary &b, pbc) -// { -// if ((&a != &b) && intersects(a,b) ) -// { -// return false; -// } -// } -// } + // // Check global intersections between boundaries + // BOOST_FOREACH(const PolygonBoundary &a, pbc) + // { + // BOOST_FOREACH(const PolygonBoundary &b, pbc) + // { + // if ((&a != &b) && intersects(a,b) ) + // { + // return false; + // } + // } + // } // Check global intersections between boundaries for (SegmentsList::const_iterator a = segments_list.begin(); a != segments_list.end(); ++a) { for (SegmentsList::const_iterator b = boost::next(a); b != segments_list.end(); ++b) { - if ( intersects(*a,*b) ) + if (intersects(*a, *b)) { return false; } @@ -258,11 +263,11 @@ bool checkBoundaryCollection(const PolygonBoundaryCollection &pbc) return true; } -std::pair closestPoint(const PolygonPt &pt, const PolygonBoundary &bnd) +std::pair closestPoint(const PolygonPt& pt, const PolygonBoundary& bnd) { size_t point_num; double close_dist2(std::numeric_limits::max()); - for (size_t idx=0, max=bnd.size()-1; idx != max; ++idx ) + for (size_t idx = 0, max = bnd.size() - 1; idx != max; ++idx) { double prox2 = pt.dist2(bnd.at(idx)); if (prox2 < close_dist2) diff --git a/godel_process_path_generation/src/process_path.cpp b/godel_process_path_generation/src/process_path.cpp index f63c1818..2b5120ee 100644 --- a/godel_process_path_generation/src/process_path.cpp +++ b/godel_process_path_generation/src/process_path.cpp @@ -34,9 +34,9 @@ visualization_msgs::Marker ProcessPath::asMarker() const visualization_msgs::Marker marker; marker.action = visualization_msgs::Marker::ADD; marker.pose.position.x = marker.pose.position.y = marker.pose.position.z = 0.; - marker.scale.x = .002; /*1mm line width*/ + marker.scale.x = .002; /*1mm line width*/ marker.type = visualization_msgs::Marker::LINE_STRIP; - for (std::vector::const_iterator pt = pts_.begin(); pt!=pts_.end(); ++pt) + for (std::vector::const_iterator pt = pts_.begin(); pt != pts_.end(); ++pt) { geometry_msgs::Point marker_pt; marker_pt.x = pt->pose().translation()(0); diff --git a/godel_process_path_generation/src/process_path_generator.cpp b/godel_process_path_generation/src/process_path_generator.cpp index 457c9e69..121054e8 100644 --- a/godel_process_path_generation/src/process_path_generator.cpp +++ b/godel_process_path_generation/src/process_path_generator.cpp @@ -31,29 +31,29 @@ using descartes::ProcessPt; - namespace godel_process_path { -void operator<<(ProcessPt &pr_pt, const PolygonPt &pg_pt) +void operator<<(ProcessPt& pr_pt, const PolygonPt& pg_pt) { pr_pt.setPosePosition(pg_pt.x, pg_pt.y, 0.); } - -void ProcessPathGenerator::addInterpolatedProcessPts(const ProcessPt &start, const ProcessPt &end, double vel) +void ProcessPathGenerator::addInterpolatedProcessPts(const ProcessPt& start, const ProcessPt& end, + double vel) { - const Eigen::Affine3d &p1 = start.pose(); - const Eigen::Affine3d &p2 = end.pose(); + const Eigen::Affine3d& p1 = start.pose(); + const Eigen::Affine3d& p2 = end.pose(); double sep = (p2.translation() - p1.translation()).norm(); if (sep > max_discretization_distance_) { - size_t new_ptcnt = static_cast(std::ceil(sep/max_discretization_distance_) - 1.); - for (size_t ii=1; ii<=new_ptcnt; ++ii) + size_t new_ptcnt = static_cast(std::ceil(sep / max_discretization_distance_) - 1.); + for (size_t ii = 1; ii <= new_ptcnt; ++ii) { - double t = static_cast(ii)/static_cast(new_ptcnt+1.); - Eigen::Vector3d pos = (1-t) * p1.translation() + t * p2.translation(); - Eigen::Quaterniond rot(Eigen::Quaterniond(p1.rotation()).slerp(t, Eigen::Quaterniond(p2.rotation()))); + double t = static_cast(ii) / static_cast(new_ptcnt + 1.); + Eigen::Vector3d pos = (1 - t) * p1.translation() + t * p2.translation(); + Eigen::Quaterniond rot( + Eigen::Quaterniond(p1.rotation()).slerp(t, Eigen::Quaterniond(p2.rotation()))); ProcessPt new_pt = start; new_pt.pose() = Eigen::Translation3d(pos) * rot; @@ -63,26 +63,25 @@ void ProcessPathGenerator::addInterpolatedProcessPts(const ProcessPt &start, con } } -void ProcessPathGenerator::addPolygonToProcessPath(const PolygonBoundary &bnd_ref, double vel) +void ProcessPathGenerator::addPolygonToProcessPath(const PolygonBoundary& bnd_ref, double vel) { PolygonBoundary bnd = bnd_ref; bnd.push_back(bnd.front()); ProcessPt process_pt; - BOOST_FOREACH(const PolygonPt &pg_pt, bnd) + BOOST_FOREACH (const PolygonPt& pg_pt, bnd) { process_pt << pg_pt; process_path_.addPoint(process_pt); process_path_.addTransition(velToTransition(vel)); } - } -void ProcessPathGenerator::addTraverseToProcessPath(const PolygonPt &from, const PolygonPt &to) +void ProcessPathGenerator::addTraverseToProcessPath(const PolygonPt& from, const PolygonPt& to) { - ProcessPt start, /*last point from previous path*/ - retract, /*safe point above previous path*/ - approach, /*safe point above next path*/ - end; /*start point of next path*/ + ProcessPt start, /*last point from previous path*/ + retract, /*safe point above previous path*/ + approach, /*safe point above next path*/ + end; /*start point of next path*/ start << from; retract.setPosePosition(from.x, from.y, safe_traverse_height_); @@ -90,22 +89,25 @@ void ProcessPathGenerator::addTraverseToProcessPath(const PolygonPt &from, const end << to; addInterpolatedProcessPts(start, retract, velocity_.retract); - process_path_.addPoint(retract); /*addInterpolatedPoints does not add endpoints */ + process_path_.addPoint(retract); /*addInterpolatedPoints does not add endpoints */ process_path_.addTransition(velToTransition(velocity_.retract)); addInterpolatedProcessPts(retract, approach, velocity_.traverse); process_path_.addPoint(approach); process_path_.addTransition(velToTransition(velocity_.traverse)); - addInterpolatedProcessPts(approach, end, velocity_.approach); //TODO Moving to 1st pt of next path is at vel.blending instead of vel.approach + addInterpolatedProcessPts(approach, end, velocity_.approach); // TODO Moving to 1st pt of next + // path is at vel.blending instead + // of vel.approach } bool ProcessPathGenerator::createProcessPath() { if (!variables_ok()) { - ROS_WARN_STREAM("Problem with variable values in ProcessPathGenerator, were they set properly?" << std::endl << - "Tool Radius: " << tool_radius_ << "(m)" << std::endl << - "Margin: " << margin_ << "(m)" << std::endl << - "Overlap: " << overlap_ << "(m)"); + ROS_WARN_STREAM("Problem with variable values in ProcessPathGenerator, were they set properly?" + << std::endl + << "Tool Radius: " << tool_radius_ << "(m)" << std::endl + << "Margin: " << margin_ << "(m)" << std::endl + << "Overlap: " << overlap_ << "(m)"); return false; } @@ -131,7 +133,7 @@ bool ProcessPathGenerator::createProcessPath() // Add approach vector process_path_.clear(); ProcessPt approach, start; - const PolygonPt &first = *(path_polygons_->begin()->begin()); + const PolygonPt& first = *(path_polygons_->begin()->begin()); approach.setPosePosition(first.x, first.y, safe_traverse_height_); start << first; @@ -142,24 +144,24 @@ bool ProcessPathGenerator::createProcessPath() // Do all loops until last double current_offset = std::numeric_limits::max(); size_t pgIdx(0); - while (pgIdx < path_polygons_->size()-1) + while (pgIdx < path_polygons_->size() - 1) { current_offset = path_offsets_->at(pgIdx); - const PolygonBoundary &polygon = path_polygons_->at(pgIdx); + const PolygonBoundary& polygon = path_polygons_->at(pgIdx); addPolygonToProcessPath(polygon, velocity_.blending); ROS_INFO_COND(verbose_, "Added polygon %li to process path.", pgIdx); - const PolygonPt last_pgpt = polygon.front(); //Each polygon ends where it starts + const PolygonPt last_pgpt = polygon.front(); // Each polygon ends where it starts ProcessPt last_pt; last_pt << last_pgpt; ++pgIdx; - PolygonBoundary &next_polygon = path_polygons_->at(pgIdx); + PolygonBoundary& next_polygon = path_polygons_->at(pgIdx); if (path_offsets_->at(pgIdx) < current_offset) - { /*Take one step out*/ + { /*Take one step out*/ size_t rotate_index = polygon_utils::closestPoint(last_pgpt, next_polygon).first; - std::rotate(next_polygon.begin(), next_polygon.begin()+rotate_index, next_polygon.end()); + std::rotate(next_polygon.begin(), next_polygon.begin() + rotate_index, next_polygon.end()); ProcessPt next_pt; next_pt << next_polygon.front(); addInterpolatedProcessPts(last_pt, next_pt, velocity_.approach); @@ -173,7 +175,7 @@ bool ProcessPathGenerator::createProcessPath() } // Add last loop and retract - const PolygonBoundary &polygon = path_polygons_->at(pgIdx); + const PolygonBoundary& polygon = path_polygons_->at(pgIdx); addPolygonToProcessPath(polygon, velocity_.blending); ROS_INFO_COND(verbose_, "Added polygon %li to process path.", pgIdx); const PolygonPt last_pgpt = polygon.front(); diff --git a/godel_process_path_generation/src/process_path_generator_node.cpp b/godel_process_path_generation/src/process_path_generator_node.cpp index 9daf6d0d..c6448468 100644 --- a/godel_process_path_generation/src/process_path_generator_node.cpp +++ b/godel_process_path_generation/src/process_path_generator_node.cpp @@ -33,39 +33,43 @@ #include "godel_process_path_generation/process_path.h" #include "godel_process_path_generation/polygon_utils.h" -const std::string OFFSET_POLYGON_SERVICE="offset_polygon"; +const std::string OFFSET_POLYGON_SERVICE = "offset_polygon"; const static double DISCRETIZATION_DISTANCE = 0.01; // m -const static double TRAVERSE_HEIGHT = 0.075; // m +const static double TRAVERSE_HEIGHT = 0.075; // m -double dist(const Eigen::Affine3d &from, const Eigen::Affine3d &to) +double dist(const Eigen::Affine3d& from, const Eigen::Affine3d& to) { - return (from.translation()-to.translation()).norm(); + return (from.translation() - to.translation()).norm(); } -bool pathDataToDurations(std::vector ×, const std::vector &pts, const std::vector &transitions) +bool pathDataToDurations(std::vector& times, + const std::vector& pts, + const std::vector& transitions) { times.clear(); - size_t n=transitions.size(); - if (0==n) + size_t n = transitions.size(); + if (0 == n) { ROS_WARN("No data to convert to timestamps."); return false; } - ROS_ASSERT_MSG(pts.size() == n+1, "Pts size %ld; Trans size %ld", n, pts.size()); + ROS_ASSERT_MSG(pts.size() == n + 1, "Pts size %ld; Trans size %ld", n, pts.size()); times.resize(n); ROS_INFO_STREAM("Creating " << n << " timestamps"); - for (size_t ii=0; iidesired); + times.at(ii) = ros::Duration(dist(pts.at(ii + 1).pose(), pts.at(ii).pose()) / vel->desired); } return true; } -bool generateProcessPlan(descartes::ProcessPath &process_path, const godel_msgs::ProcessPlanningRequest &req, ros::ServiceClientPtr offset_service_client) +bool generateProcessPlan(descartes::ProcessPath& process_path, + const godel_msgs::ProcessPlanningRequest& req, + ros::ServiceClientPtr offset_service_client) { // Create ProcessPathGenerator and initialize. godel_process_path::ProcessPathGenerator ppg; @@ -92,7 +96,7 @@ bool generateProcessPlan(descartes::ProcessPath &process_path, const godel_msgs: // Call polygon_offset service. godel_msgs::OffsetBoundaryRequest ob_req; godel_msgs::OffsetBoundaryResponse ob_res; - + ob_req.discretization = DISCRETIZATION_DISTANCE; ob_req.initial_offset = req.params.tool_radius + req.params.margin; ob_req.offset_distance = req.params.tool_radius - req.params.overlap; @@ -122,8 +126,7 @@ bool generateProcessPlan(descartes::ProcessPath &process_path, const godel_msgs: return true; } -bool pathGen(godel_msgs::ProcessPlanningRequest &req, - godel_msgs::ProcessPlanningResponse &res, +bool pathGen(godel_msgs::ProcessPlanningRequest& req, godel_msgs::ProcessPlanningResponse& res, ros::ServiceClientPtr offset_service_client) { ROS_WARN("pathGen not currently implemented."); @@ -134,12 +137,12 @@ bool pathGen(godel_msgs::ProcessPlanningRequest &req, generateProcessPlan(process_path, req, offset_service_client); // Translate ProcessPath into ProcessPathMsg - //TODO msg not defined + // TODO msg not defined return true; } -bool pathGenVisual(godel_process_path_generation::VisualizeBlendingPlanRequest &req, - godel_process_path_generation::VisualizeBlendingPlanResponse &res, +bool pathGenVisual(godel_process_path_generation::VisualizeBlendingPlanRequest& req, + godel_process_path_generation::VisualizeBlendingPlanResponse& res, ros::ServiceClientPtr offset_service_client) { // Call function to generate process path. @@ -163,27 +166,29 @@ bool pathGenVisual(godel_process_path_generation::VisualizeBlendingPlanRequest & return true; } -int main(int argc, char **argv) +int main(int argc, char** argv) { ros::init(argc, argv, "process_path_generator"); ros::NodeHandle nh; // waiting for service - while(!ros::service::waitForService(OFFSET_POLYGON_SERVICE,ros::Duration(10.0f))) + while (!ros::service::waitForService(OFFSET_POLYGON_SERVICE, ros::Duration(10.0f))) { - ROS_WARN_STREAM("Connecting to service '"<(OFFSET_POLYGON_SERVICE))); - ros::ServiceServer visualize_path_generator = nh.advertiseService - ("visualize_path_generator", boost::bind(pathGenVisual, _1, _2, boundary_offset_client)); + ros::ServiceClientPtr boundary_offset_client( + new ros::ServiceClient(nh.serviceClient(OFFSET_POLYGON_SERVICE))); + ros::ServiceServer visualize_path_generator = + nh.advertiseService( + "visualize_path_generator", boost::bind(pathGenVisual, _1, _2, boundary_offset_client)); ROS_INFO("%s ready to service requests.", visualize_path_generator.getService().c_str()); - ros::ServiceServer path_generator = nh.advertiseService - ("process_path_generator", boost::bind(pathGen, _1, _2, boundary_offset_client)); + ros::ServiceServer path_generator = + nh.advertiseService( + "process_path_generator", boost::bind(pathGen, _1, _2, boundary_offset_client)); ROS_INFO("%s ready to service requests.", path_generator.getService().c_str()); ros::spin(); diff --git a/godel_process_path_generation/src/process_path_visualization_node.cpp b/godel_process_path_generation/src/process_path_visualization_node.cpp index 3336ae96..116a8c54 100644 --- a/godel_process_path_generation/src/process_path_visualization_node.cpp +++ b/godel_process_path_generation/src/process_path_visualization_node.cpp @@ -32,7 +32,6 @@ #include "godel_process_path_generation/VisualizeBlendingPlan.h" #include "godel_process_path_generation/utils.h" - const float TOOL_DIA = .050; const float TOOL_THK = .005; const float TOOL_SHAFT_DIA = .006; @@ -41,13 +40,13 @@ const float MARGIN = .005; const float PATH_OVERLAP = .01; const float DISCRETIZATION = .0025; - -visualization_msgs::MarkerArray toolMarker(const geometry_msgs::Point &pos, const geometry_msgs::Pose &pose) +visualization_msgs::MarkerArray toolMarker(const geometry_msgs::Point& pos, + const geometry_msgs::Pose& pose) { visualization_msgs::MarkerArray tool; tool.markers.resize(2); - visualization_msgs::Marker &disk = tool.markers.at(0); - visualization_msgs::Marker &shaft = tool.markers.at(1); + visualization_msgs::Marker& disk = tool.markers.at(0); + visualization_msgs::Marker& shaft = tool.markers.at(1); std_msgs::ColorRGBA blue; blue.r = 0.; @@ -71,14 +70,14 @@ visualization_msgs::MarkerArray toolMarker(const geometry_msgs::Point &pos, cons tf::poseMsgToTF(pose, marker_pose); disk.id = 0; - tf::Vector3 marker_pos(pos.x, pos.y, pos.z + .5*TOOL_THK); + tf::Vector3 marker_pos(pos.x, pos.y, pos.z + .5 * TOOL_THK); marker_pos = marker_pose * marker_pos; tf::pointTFToMsg(marker_pos, disk.pose.position); disk.scale.x = disk.scale.y = TOOL_DIA; disk.scale.z = TOOL_THK; shaft.id = 1; - marker_pos = tf::Vector3(pos.x, pos.y, pos.z + TOOL_THK + 0.5*TOOL_SHAFT_LEN); + marker_pos = tf::Vector3(pos.x, pos.y, pos.z + TOOL_THK + 0.5 * TOOL_SHAFT_LEN); marker_pos = marker_pose * marker_pos; tf::pointTFToMsg(marker_pos, shaft.pose.position); shaft.scale.x = shaft.scale.y = TOOL_SHAFT_DIA; @@ -87,13 +86,12 @@ visualization_msgs::MarkerArray toolMarker(const geometry_msgs::Point &pos, cons return tool; } -int main(int argc, char **argv) +int main(int argc, char** argv) { - boost::program_options::options_description desc("This node publishes a process path and animates a tool moving along it."); - desc.add_options() - ("help", "produce help message") - ("demo", "run in demo-mode (does not contact other nodes)") - ; + boost::program_options::options_description desc( + "This node publishes a process path and animates a tool moving along it."); + desc.add_options()("help", "produce help message")( + "demo", "run in demo-mode (does not contact other nodes)"); boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); @@ -101,13 +99,15 @@ int main(int argc, char **argv) if (vm.count("help")) { - std::cout << desc << "\n"; - return 0; + std::cout << desc << "\n"; + return 0; } ros::init(argc, argv, "process_path_visualization"); ros::NodeHandle nh; - ros::ServiceClient path_generator_client=nh.serviceClient("visualize_path_generator"); + ros::ServiceClient path_generator_client = + nh.serviceClient( + "visualize_path_generator"); ros::Publisher path_pub = nh.advertise("process_path", 1, true); ros::Publisher tool_pub = nh.advertise("sanding_tool", 1, true); ROS_INFO_STREAM("Started node '" << ros::this_node::getName() << "'"); @@ -128,32 +128,48 @@ int main(int argc, char **argv) bp_req.params.margin = MARGIN; bp_req.params.overlap = PATH_OVERLAP; bp_req.params.safe_traverse_height = .05; - bp_req.params.tool_radius = TOOL_DIA/2.; + bp_req.params.tool_radius = TOOL_DIA / 2.; // Pose of pocket surface geometry_msgs::Pose pocket_pose_msg; // Populate request for boundary offsets - std::vector &boundaries = bp_req.surface.boundaries; + std::vector& boundaries = bp_req.surface.boundaries; if (vm.count("demo")) { ROS_INFO("Running in demo mode"); // Outer boundary geometry_msgs::Point32 pt; geometry_msgs::Polygon boundary; - pt.x = 0.; pt.y = 0.; boundary.points.push_back(pt); - pt.x = .5; pt.y = 0.; boundary.points.push_back(pt); - pt.x = .5; pt.y = .5; boundary.points.push_back(pt); -// pt.x = .25; pt.y = .125; boundary.points.push_back(pt); - pt.x = .25; pt.y = .45; boundary.points.push_back(pt); - pt.x = 0.; pt.y = .5; boundary.points.push_back(pt); + pt.x = 0.; + pt.y = 0.; + boundary.points.push_back(pt); + pt.x = .5; + pt.y = 0.; + boundary.points.push_back(pt); + pt.x = .5; + pt.y = .5; + boundary.points.push_back(pt); + // pt.x = .25; pt.y = .125; boundary.points.push_back(pt); + pt.x = .25; + pt.y = .45; + boundary.points.push_back(pt); + pt.x = 0.; + pt.y = .5; + boundary.points.push_back(pt); boundaries.push_back(boundary); // Inner boundary (can only use with .25,.45 not with .25,.125) boundary.points.clear(); - pt.x = .3; pt.y = .1; boundary.points.push_back(pt); - pt.x = .2; pt.y = .1; boundary.points.push_back(pt); - pt.x = .25; pt.y = .2; boundary.points.push_back(pt); + pt.x = .3; + pt.y = .1; + boundary.points.push_back(pt); + pt.x = .2; + pt.y = .1; + boundary.points.push_back(pt); + pt.x = .25; + pt.y = .2; + boundary.points.push_back(pt); boundaries.push_back(boundary); // Pose of pocket surface @@ -172,9 +188,9 @@ int main(int argc, char **argv) } // waiting for service - while(!path_generator_client.waitForExistence(ros::Duration(10))) + while (!path_generator_client.waitForExistence(ros::Duration(10))) { - ROS_WARN_STREAM("Connecting to Service "< max.at(ii) || min.at(ii) > desired.at(ii) || desired.at(ii) > max.at(ii)) { @@ -62,6 +61,4 @@ bool RotationalVelocityConstraint::isValid() return true; } - - } /* namespace descartes */ diff --git a/godel_process_path_generation/test/test_MeshImporter.cpp b/godel_process_path_generation/test/test_MeshImporter.cpp index fb16f1a4..a21d11c5 100644 --- a/godel_process_path_generation/test/test_MeshImporter.cpp +++ b/godel_process_path_generation/test/test_MeshImporter.cpp @@ -20,15 +20,13 @@ using godel_process_path::PolygonPt; using godel_process_path::PolygonBoundary; using godel_process_path::PolygonBoundaryCollection; - -class MeshImporterTest: public ::testing::Test +class MeshImporterTest : public ::testing::Test { protected: - // x,y,z = 0,1,2 void createMesh(int dir) { - ROS_ASSERT(dir==0 || dir==1 || dir==2); + ROS_ASSERT(dir == 0 || dir == 1 || dir == 2); pointcloud_ = CloudN::Ptr(new CloudN); mesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); @@ -40,62 +38,115 @@ class MeshImporterTest: public ::testing::Test * | 2 | * |/ \| * 3---4 */ - p0.normal_x = (dir==0)?1:0; p0.normal_y = (dir==1)?1:0; p0.normal_z = (dir==2)?1:0; - p4=p3=p2=p1=p0; + p0.normal_x = (dir == 0) ? 1 : 0; + p0.normal_y = (dir == 1) ? 1 : 0; + p0.normal_z = (dir == 2) ? 1 : 0; + p4 = p3 = p2 = p1 = p0; - if (dir == 0) //normal in X-direction + if (dir == 0) // normal in X-direction { - p0.x=0; p0.y=1; p0.z=1; pointcloud_->push_back(p0); - p1.x=0; p1.y=1; p1.z=-1; pointcloud_->push_back(p1); - p2.x=0; p2.y=0; p2.z=0; pointcloud_->push_back(p2); - p3.x=0; p3.y=-1; p3.z=1; pointcloud_->push_back(p3); - p4.x=0; p4.y=-1; p4.z=-1; pointcloud_->push_back(p4); + p0.x = 0; + p0.y = 1; + p0.z = 1; + pointcloud_->push_back(p0); + p1.x = 0; + p1.y = 1; + p1.z = -1; + pointcloud_->push_back(p1); + p2.x = 0; + p2.y = 0; + p2.z = 0; + pointcloud_->push_back(p2); + p3.x = 0; + p3.y = -1; + p3.z = 1; + pointcloud_->push_back(p3); + p4.x = 0; + p4.y = -1; + p4.z = -1; + pointcloud_->push_back(p4); } - else if (dir == 1) // normal in Y-direction + else if (dir == 1) // normal in Y-direction { - p0.x=-1; p0.y=0; p0.z=1; pointcloud_->push_back(p0); - p1.x=-1; p1.y=0; p1.z=-1; pointcloud_->push_back(p1); - p2.x=0; p2.y=0; p2.z=0; pointcloud_->push_back(p2); - p3.x=1; p3.y=0; p3.z=1; pointcloud_->push_back(p3); - p4.x=1; p4.y=0; p4.z=-1; pointcloud_->push_back(p4); + p0.x = -1; + p0.y = 0; + p0.z = 1; + pointcloud_->push_back(p0); + p1.x = -1; + p1.y = 0; + p1.z = -1; + pointcloud_->push_back(p1); + p2.x = 0; + p2.y = 0; + p2.z = 0; + pointcloud_->push_back(p2); + p3.x = 1; + p3.y = 0; + p3.z = 1; + pointcloud_->push_back(p3); + p4.x = 1; + p4.y = 0; + p4.z = -1; + pointcloud_->push_back(p4); } - else // normal in Z-direction + else // normal in Z-direction { - p0.x=-1; p0.y=1; p0.z=0; pointcloud_->push_back(p0); - p1.x=1; p1.y=1; p1.z=0; pointcloud_->push_back(p1); - p2.x=0; p2.y=0; p2.z=0; pointcloud_->push_back(p2); - p3.x=-1; p3.y=-1; p3.z=0; pointcloud_->push_back(p3); - p4.x=1; p4.y=-1; p4.z=0; pointcloud_->push_back(p4); + p0.x = -1; + p0.y = 1; + p0.z = 0; + pointcloud_->push_back(p0); + p1.x = 1; + p1.y = 1; + p1.z = 0; + pointcloud_->push_back(p1); + p2.x = 0; + p2.y = 0; + p2.z = 0; + pointcloud_->push_back(p2); + p3.x = -1; + p3.y = -1; + p3.z = 0; + pointcloud_->push_back(p3); + p4.x = 1; + p4.y = -1; + p4.z = 0; + pointcloud_->push_back(p4); } - expected_boundary_.push_back(PolygonPt(-1,-1)); + expected_boundary_.push_back(PolygonPt(-1, -1)); expected_boundary_.push_back(PolygonPt(-1, 1)); - expected_boundary_.push_back(PolygonPt( 1, 1)); - expected_boundary_.push_back(PolygonPt( 1,-1)); + expected_boundary_.push_back(PolygonPt(1, 1)); + expected_boundary_.push_back(PolygonPt(1, -1)); pcl::toPCLPointCloud2(*pointcloud_, mesh_->cloud); pcl::Vertices polygon; polygon.vertices.clear(); - polygon.vertices.push_back(0); polygon.vertices.push_back(2); polygon.vertices.push_back(1); + polygon.vertices.push_back(0); + polygon.vertices.push_back(2); + polygon.vertices.push_back(1); mesh_->polygons.push_back(polygon); polygon.vertices.clear(); - polygon.vertices.push_back(1); polygon.vertices.push_back(2); polygon.vertices.push_back(4); + polygon.vertices.push_back(1); + polygon.vertices.push_back(2); + polygon.vertices.push_back(4); mesh_->polygons.push_back(polygon); polygon.vertices.clear(); - polygon.vertices.push_back(4); polygon.vertices.push_back(2); polygon.vertices.push_back(3); + polygon.vertices.push_back(4); + polygon.vertices.push_back(2); + polygon.vertices.push_back(3); mesh_->polygons.push_back(polygon); polygon.vertices.clear(); - polygon.vertices.push_back(3); polygon.vertices.push_back(2); polygon.vertices.push_back(0); + polygon.vertices.push_back(3); + polygon.vertices.push_back(2); + polygon.vertices.push_back(0); mesh_->polygons.push_back(polygon); } - virtual void SetUp() - { - } + virtual void SetUp() {} CloudN::Ptr pointcloud_; pcl::PolygonMeshPtr mesh_; @@ -118,8 +169,8 @@ TEST_F(calcBoundaryData, fixedMeshes) PolygonBoundary actual = actual_collection.front(); EXPECT_TRUE(::isCircularPermutation(actual, expected_boundary_)); -// std::cout << "Actual:\n" << actual << std::endl << "Expected:\n" << expected_boundary_ << std::endl; - + // std::cout << "Actual:\n" << actual << std::endl << "Expected:\n" << expected_boundary_ << + // std::endl; // Flat mesh with Normal along X createMesh(0); @@ -129,7 +180,8 @@ TEST_F(calcBoundaryData, fixedMeshes) actual = actual_collection.front(); EXPECT_TRUE(::isCircularPermutation(actual, expected_boundary_)); -// std::cout << "Actual:\n" << actual << std::endl << "Expected:\n" << expected_boundary_ << std::endl; + // std::cout << "Actual:\n" << actual << std::endl << "Expected:\n" << expected_boundary_ << + // std::endl; // Flat mesh with Normal along Y createMesh(1); @@ -139,7 +191,8 @@ TEST_F(calcBoundaryData, fixedMeshes) actual = actual_collection.front(); EXPECT_TRUE(::isCircularPermutation(actual, expected_boundary_)); -// std::cout << "Actual:\n" << actual << std::endl << "Expected:\n" << expected_boundary_ << std::endl; + // std::cout << "Actual:\n" << actual << std::endl << "Expected:\n" << expected_boundary_ << + // std::endl; } TEST_F(badInput, nonTriangularMesh) @@ -159,7 +212,7 @@ TEST_F(badInput, nonTriangularMesh) EXPECT_FALSE(mi.calculateBoundaryData(*mesh_)); } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/godel_process_path_generation/test/test_ProcessPathGenerator.cpp b/godel_process_path_generation/test/test_ProcessPathGenerator.cpp index da220ca7..be1de908 100644 --- a/godel_process_path_generation/test/test_ProcessPathGenerator.cpp +++ b/godel_process_path_generation/test/test_ProcessPathGenerator.cpp @@ -15,30 +15,30 @@ TEST(ProcessPathGeneratorTest, init) { ProcessPathGenerator ppg; ppg.setTraverseHeight(.05); - EXPECT_FALSE(ppg.createProcessPath()); // variables not properly set + EXPECT_FALSE(ppg.createProcessPath()); // variables not properly set ppg.setMargin(-.01); ppg.setOverlap(.01); ppg.setToolRadius(.02); - EXPECT_FALSE(ppg.createProcessPath()); // variables not properly set (margin) + EXPECT_FALSE(ppg.createProcessPath()); // variables not properly set (margin) ppg.setMargin(0.); ppg.setOverlap(.04); ppg.setToolRadius(.02); - EXPECT_FALSE(ppg.createProcessPath()); // variables not properly set (overlap) + EXPECT_FALSE(ppg.createProcessPath()); // variables not properly set (overlap) ppg.setMargin(.005); ppg.setOverlap(.01); ppg.setToolRadius(.025); - EXPECT_FALSE(ppg.createProcessPath()); // Configure not done + EXPECT_FALSE(ppg.createProcessPath()); // Configure not done // Create single PolygonBoundary -// godel_process_path::PolygonBoundaryCollection boundaries; + // godel_process_path::PolygonBoundaryCollection boundaries; godel_process_path::PolygonBoundary boundary; boundary.push_back(PolygonPt(0., 0.)); boundary.push_back(PolygonPt(.1, 0.)); boundary.push_back(PolygonPt(0., .1)); ppg.verbose_ = true; godel_process_path::PolygonBoundaryCollection boundaries(1, boundary); - std::vector offsets(1,.03); // Offset = tool radius + margin + std::vector offsets(1, .03); // Offset = tool radius + margin EXPECT_TRUE(ppg.setPathPolygons(&boundaries, &offsets)); EXPECT_FALSE(ppg.createProcessPath()); @@ -67,12 +67,12 @@ TEST(ProcessPathGeneratorTest, complete) boundary.push_back(PolygonPt(.0, .5)); ppg.verbose_ = true; godel_process_path::PolygonBoundaryCollection boundaries(1, boundary); - std::vector offsets(1,.03); // Offset = tool radius + margin + std::vector offsets(1, .03); // Offset = tool radius + margin EXPECT_TRUE(ppg.setPathPolygons(&boundaries, &offsets)); EXPECT_TRUE(ppg.createProcessPath()); } -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/godel_process_path_generation/test/test_common_functions.h b/godel_process_path_generation/test/test_common_functions.h index 56e85154..34ba2bb6 100644 --- a/godel_process_path_generation/test/test_common_functions.h +++ b/godel_process_path_generation/test/test_common_functions.h @@ -28,87 +28,90 @@ #include -namespace /*alternate license*/ +namespace /*alternate license*/ { - /* - * Note: The code in this block is under a different license than the rest of the file! - * - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2009-2012, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. - * - * 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 copyright holder(s) 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. - * - * $Id$ - * - */ +/* +* Note: The code in this block is under a different license than the rest of the file! +* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2009-2012, Willow Garage, Inc. +* Copyright (c) 2012-, Open Perception, Inc. +* +* 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 copyright holder(s) 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. +* +* $Id$ +* +*/ /** \brief Check if the 'actual' is a circular permutation of 'expected' (only clockwise). * \example [0 1 2 3] [1 2 3 0] [2 3 0 1] [3 0 1 2] are all equal. */ -template bool -isCircularPermutation (const ContainerT& expected, const ContainerT& actual, const bool verbose = false) +template +bool isCircularPermutation(const ContainerT& expected, const ContainerT& actual, + const bool verbose = false) { - const unsigned int n = static_cast (expected.size ()); - EXPECT_EQ (n, actual.size ()); - if (n != actual.size ()) + const unsigned int n = static_cast(expected.size()); + EXPECT_EQ(n, actual.size()); + if (n != actual.size()) { - if (verbose) std::cerr << "expected.size () != actual.size (): " << n << " != " << actual.size () << "\n"; + if (verbose) + std::cerr << "expected.size () != actual.size (): " << n << " != " << actual.size() << "\n"; return (false); } - for (unsigned int i=0; i #include "godel_process_path_generation/polygon_utils.h" - TEST(PolygonSegment, simple) { /* @@ -35,17 +34,10 @@ TEST(PolygonSegment, simple) * |/ * p3 * */ - godel_process_path::PolygonPt p0(0., 0.), - p1(1., 0.), - p2(0., 1.), - p3(0., -1), - p4(1., 1.); + godel_process_path::PolygonPt p0(0., 0.), p1(1., 0.), p2(0., 1.), p3(0., -1), p4(1., 1.); - godel_process_path::polygon_utils::PolygonSegment - s01(p0,p1), - s02(p0,p2), - s23(p2,p3), - s34(p3,p4); + godel_process_path::polygon_utils::PolygonSegment s01(p0, p1), s02(p0, p2), s23(p2, p3), + s34(p3, p4); EXPECT_TRUE(s01.intersects(s23)); EXPECT_TRUE(s01.intersects(s02)); @@ -61,27 +53,18 @@ TEST(PolygonSegment, complex) * // * * p4 */ - godel_process_path::PolygonPt p0(0.035726, 0.017804), - p1(0.038726, 0.021804), - p2(0.038726, 0.017804), - p3(0.032726, 0.017804), - p4(0.038726, 0.014804); + godel_process_path::PolygonPt p0(0.035726, 0.017804), p1(0.038726, 0.021804), + p2(0.038726, 0.017804), p3(0.032726, 0.017804), p4(0.038726, 0.014804); - godel_process_path::polygon_utils::PolygonSegment - s01(p0,p1), - s12(p1,p2), - s23(p2,p3), - s34(p3,p4), - s40(p4,p0), - s41(p4,p1); + godel_process_path::polygon_utils::PolygonSegment s01(p0, p1), s12(p1, p2), s23(p2, p3), + s34(p3, p4), s40(p4, p0), s41(p4, p1); EXPECT_FALSE(s01.intersects(s34)); EXPECT_TRUE(s01.intersects(s23, .001)); EXPECT_TRUE(s41.intersects(s23)); } - -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/godel_process_planning/include/godel_process_planning/godel_process_planning.h b/godel_process_planning/include/godel_process_planning/godel_process_planning.h index f6c3d397..b5bd1481 100644 --- a/godel_process_planning/include/godel_process_planning/godel_process_planning.h +++ b/godel_process_planning/include/godel_process_planning/godel_process_planning.h @@ -41,11 +41,11 @@ class ProcessPlanningManager descartes_core::RobotModelPtr blend_model_; descartes_core::RobotModelPtr keyence_model_; moveit::core::RobotModelConstPtr moveit_model_; - pluginlib::ClassLoader plugin_loader_; // kept around so code doesn't get unloaded + pluginlib::ClassLoader + plugin_loader_; // kept around so code doesn't get unloaded std::string blend_group_name_; std::string keyence_group_name_; }; - } #endif diff --git a/godel_process_planning/src/blend_process_planning.cpp b/godel_process_planning/src/blend_process_planning.cpp index f50f6a44..556f6e2f 100644 --- a/godel_process_planning/src/blend_process_planning.cpp +++ b/godel_process_planning/src/blend_process_planning.cpp @@ -10,23 +10,27 @@ #include "common_utils.h" #include "boost/make_shared.hpp" - namespace godel_process_planning { // Planning Constants -const double BLENDING_ANGLE_DISCRETIZATION = M_PI / 12.0; // The discretization of the tool's pose about - // the z axis -const double FREE_SPACE_MAX_ANGLE_DELTA = M_PI_2; // The maximum angle a joint during a freespace motion - // from the start to end position without that motion - // being penalized. Avoids flips. -const double FREE_SPACE_ANGLE_PENALTY = 5.0; // The factor by which a joint motion is multiplied if said - // motion is greater than the max. -const static std::string JOINT_TOPIC_NAME = "joint_states"; // ROS topic to subscribe to for current robot - // state info +const double BLENDING_ANGLE_DISCRETIZATION = + M_PI / 12.0; // The discretization of the tool's pose about + // the z axis +const double FREE_SPACE_MAX_ANGLE_DELTA = + M_PI_2; // The maximum angle a joint during a freespace motion + // from the start to end position without that motion + // being penalized. Avoids flips. +const double FREE_SPACE_ANGLE_PENALTY = + 5.0; // The factor by which a joint motion is multiplied if said + // motion is greater than the max. +const static std::string JOINT_TOPIC_NAME = + "joint_states"; // ROS topic to subscribe to for current robot + // state info /** - * @brief Translated an Eigen pose to a Descartes trajectory point appropriate for the BLEND process! + * @brief Translated an Eigen pose to a Descartes trajectory point appropriate for the BLEND + * process! * Note that this function is local only to this file, and there is a similar function in * the keyence_process_planning.cpp document. * @param pose @@ -37,7 +41,7 @@ static inline descartes_core::TrajectoryPtPtr toDescartesPt(const Eigen::Affine3 { using namespace descartes_trajectory; using namespace descartes_core; - const TimingConstraint tm (dt); + const TimingConstraint tm(dt); return boost::make_shared(pose, BLENDING_ANGLE_DISCRETIZATION, AxialSymmetricPt::Z_AXIS, tm); } @@ -67,20 +71,23 @@ static inline double freeSpaceCostFunction(const std::vector& source, } /** - * @brief transforms an input, in the form of a reference pose and points relative to that pose, into Descartes' + * @brief transforms an input, in the form of a reference pose and points relative to that pose, + * into Descartes' * native format. Also adds in associated parameters. - * @param ref The reference posed that all points are multiplied by. Should be in the world space of the blend move group. + * @param ref The reference posed that all points are multiplied by. Should be in the world space of + * the blend move group. * @param points Sequence of points (relative to ref and the world space of blending robot model) * @param params Surface blending parameters, including info such as traversal speed * @return The input trajectory encoded in Descartes points */ -static godel_process_planning::DescartesTraj toDescartesTraj(const geometry_msgs::Pose& ref, - const std::vector& points, - const godel_msgs::BlendingPlanParameters& params) +static godel_process_planning::DescartesTraj +toDescartesTraj(const geometry_msgs::Pose& ref, const std::vector& points, + const godel_msgs::BlendingPlanParameters& params) { DescartesTraj traj; traj.reserve(points.size()); - if (points.empty()) return traj; + if (points.empty()) + return traj; Eigen::Affine3d last_pose = createNominalTransform(ref, points.front()); @@ -90,7 +97,7 @@ static godel_process_planning::DescartesTraj toDescartesTraj(const geometry_msgs // O(1) jerky - may need to revisit this time parameterization later. This at least allows // Descartes to perform some optimizations in its graph serach. double dt = (this_pose.translation() - last_pose.translation()).norm() / params.traverse_spd; - traj.push_back( toDescartesPt(this_pose, dt) ); + traj.push_back(toDescartesPt(this_pose, dt)); last_pose = this_pose; } @@ -110,7 +117,7 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin // Precondition: Input trajectory must be non-zero if (req.path.points.empty()) { - ROS_WARN("%s: Cannot create blend process plan for empty trajectory", __FUNCTION__); + ROS_WARN("%s: Cannot create blend process plan for empty trajectory", __FUNCTION__); return false; } @@ -147,16 +154,20 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin blend_model_->getFK(current_joints, init_pose); // Compute the nominal tool pose of the final process point Eigen::Affine3d process_stop_pose; - process_points.back()->getNominalCartPose(std::vector(), *blend_model_, process_stop_pose); + process_points.back()->getNominalCartPose(std::vector(), *blend_model_, + process_stop_pose); - // Joint interpolate from the initial robot position to 'best' starting configuration of process path + // Joint interpolate from the initial robot position to 'best' starting configuration of process + // path DescartesTraj to_process = createJointPath(current_joints, start_joint_poses[best_index]); - to_process.front() = descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); + to_process.front() = + descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); // To get a rough estimate of process path cost, add a cartesian move from the final process point // to the starting position again. DescartesTraj from_process = createLinearPath(process_stop_pose, init_pose); - from_process.back() = descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); + from_process.back() = + descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); // Affix the approach and depart paths calculated above with user process path DescartesTraj seed_path; @@ -175,16 +186,19 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin // 2. Use MoveIt (RRT-Connect) if the above fails // This is the only portion of the trajectory that is collision checked. Note this // method also converts the Descartes points into ROS trajectories. - trajectory_msgs::JointTrajectory approach = planFreeMove(*blend_model_, blend_group_name_, moveit_model_, - extractJoints(*blend_model_, *solved_path[0]), - extractJoints(*blend_model_, *solved_path[to_process.size()])); + trajectory_msgs::JointTrajectory approach = + planFreeMove(*blend_model_, blend_group_name_, moveit_model_, + extractJoints(*blend_model_, *solved_path[0]), + extractJoints(*blend_model_, *solved_path[to_process.size()])); - trajectory_msgs::JointTrajectory depart = planFreeMove(*blend_model_, blend_group_name_, moveit_model_, - extractJoints(*blend_model_, *solved_path[to_process.size() + process_points.size() -1]), - extractJoints(*blend_model_, *solved_path[seed_path.size() - 1])); + trajectory_msgs::JointTrajectory depart = planFreeMove( + *blend_model_, blend_group_name_, moveit_model_, + extractJoints(*blend_model_, *solved_path[to_process.size() + process_points.size() - 1]), + extractJoints(*blend_model_, *solved_path[seed_path.size() - 1])); // Break out the process path from the seed path and convert to ROS messages - DescartesTraj process_part (solved_path.begin() + to_process.size(), solved_path.end() - from_process.size()); + DescartesTraj process_part(solved_path.begin() + to_process.size(), + solved_path.end() - from_process.size()); trajectory_msgs::JointTrajectory process = toROSTrajectory(process_part, *blend_model_); // Fill in result trajectories @@ -193,7 +207,7 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin res.plan.trajectory_depart = depart; // Fill in result header information - const std::vector< std::string >& joint_names = + const std::vector& joint_names = moveit_model_->getJointModelGroup(blend_group_name_)->getActiveJointModelNames(); godel_process_planning::fillTrajectoryHeaders(joint_names, res.plan.trajectory_approach); @@ -205,5 +219,4 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin return true; } - } diff --git a/godel_process_planning/src/common_utils.cpp b/godel_process_planning/src/common_utils.cpp index cfe2b39d..9d1f610f 100644 --- a/godel_process_planning/src/common_utils.cpp +++ b/godel_process_planning/src/common_utils.cpp @@ -11,19 +11,22 @@ #include "trajectory_utils.h" // Constants -const static double DEFAULT_TIME_UNDEFINED_VELOCITY = 0.1; // When a Descartes trajectory point has no timing info associated - // with it, this value (in seconds) is used instead -const static std::string DEFAULT_FRAME_ID = "world_frame"; // The default frame_id used for trajectories generated - // by these helper functions -const static double DEFAULT_ANGLE_DISCRETIZATION = M_PI / 12.0; // Default discretization used for axially-symmetric points - // in these helper functions -const static double DEFAULT_JOINT_WAIT_TIME = 5.0; // Maximum time allowed to capture a new joint - // state message +const static double DEFAULT_TIME_UNDEFINED_VELOCITY = + 0.1; // When a Descartes trajectory point has no timing info associated + // with it, this value (in seconds) is used instead +const static std::string DEFAULT_FRAME_ID = + "world_frame"; // The default frame_id used for trajectories generated + // by these helper functions +const static double DEFAULT_ANGLE_DISCRETIZATION = + M_PI / 12.0; // Default discretization used for axially-symmetric points + // in these helper functions +const static double DEFAULT_JOINT_WAIT_TIME = 5.0; // Maximum time allowed to capture a new joint + // state message // MoveIt Configuration Constants const static int DEFAULT_MOVEIT_NUM_PLANNING_ATTEMPTS = 20; -const static double DEFAULT_MOVEIT_PLANNING_TIME = 20.0; // seconds -const static double DEFAULT_MOVEIT_VELOCITY_SCALING = 0.1; // Slow down the robot +const static double DEFAULT_MOVEIT_PLANNING_TIME = 20.0; // seconds +const static double DEFAULT_MOVEIT_VELOCITY_SCALING = 0.1; // Slow down the robot const static std::string DEFAULT_MOVEIT_PLANNER_ID = "RRTConnectkConfigDefault"; const static std::string DEFAULT_MOVEIT_FRAME_ID = "world_frame"; const static std::string DEFAULT_MOVEIT_PLANNING_SERVICE_NAME = "plan_kinematic_path"; @@ -53,7 +56,7 @@ bool godel_process_planning::descartesSolve(const godel_process_planning::Descar godel_process_planning::DescartesTraj& out_path) { // Create planner - descartes_core::PathPlannerBasePtr planner (new descartes_planner::DensePlanner); + descartes_core::PathPlannerBasePtr planner(new descartes_planner::DensePlanner); if (!planner->initialize(robot_model)) { ROS_ERROR("%s: Failed to initialize planner with robot model", __FUNCTION__); @@ -95,8 +98,10 @@ godel_process_planning::toROSTrajectory(const godel_process_planning::DescartesT pt.effort.resize(joint_point.size(), 0.0); double time_step = solution[i]->getTiming().upper; // request descartes timing - if (time_step == 0.0) from_start += ros::Duration(DEFAULT_TIME_UNDEFINED_VELOCITY); // default time - else from_start += ros::Duration(time_step); + if (time_step == 0.0) + from_start += ros::Duration(DEFAULT_TIME_UNDEFINED_VELOCITY); // default time + else + from_start += ros::Duration(time_step); pt.time_from_start = from_start; @@ -107,7 +112,7 @@ godel_process_planning::toROSTrajectory(const godel_process_planning::DescartesT } void godel_process_planning::fillTrajectoryHeaders(const std::vector& joints, - trajectory_msgs::JointTrajectory& traj) + trajectory_msgs::JointTrajectory& traj) { traj.joint_names = joints; traj.header.frame_id = DEFAULT_FRAME_ID; @@ -116,14 +121,15 @@ void godel_process_planning::fillTrajectoryHeaders(const std::vector godel_process_planning::getCurrentJointState(const std::string& topic) { - sensor_msgs::JointStateConstPtr state = ros::topic::waitForMessage(topic, ros::Duration(DEFAULT_JOINT_WAIT_TIME)); - if (!state) throw std::runtime_error("Joint state message capture failed"); + sensor_msgs::JointStateConstPtr state = ros::topic::waitForMessage( + topic, ros::Duration(DEFAULT_JOINT_WAIT_TIME)); + if (!state) + throw std::runtime_error("Joint state message capture failed"); return state->position; } godel_process_planning::DescartesTraj -godel_process_planning::createLinearPath(const Eigen::Affine3d &start, - const Eigen::Affine3d &stop, +godel_process_planning::createLinearPath(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, double ds) { using namespace descartes_trajectory; @@ -132,19 +138,17 @@ godel_process_planning::createLinearPath(const Eigen::Affine3d &start, DescartesTraj result; for (std::size_t i = 0; i < cart_path.size(); ++i) { - result.push_back(boost::make_shared(cart_path[i], - DEFAULT_ANGLE_DISCRETIZATION, - descartes_trajectory::AxialSymmetricPt::Z_AXIS)); + result.push_back( + boost::make_shared(cart_path[i], DEFAULT_ANGLE_DISCRETIZATION, + descartes_trajectory::AxialSymmetricPt::Z_AXIS)); } return result; } - godel_process_planning::DescartesTraj -godel_process_planning::createJointPath(const std::vector &start, - const std::vector &stop, - double dtheta) +godel_process_planning::createJointPath(const std::vector& start, + const std::vector& stop, double dtheta) { JointVector path = interpolateJoint(start, stop, dtheta); DescartesTraj result; @@ -155,11 +159,9 @@ godel_process_planning::createJointPath(const std::vector &start, return result; } - -trajectory_msgs::JointTrajectory godel_process_planning::getMoveitPlan(const std::string &group_name, - const std::vector &joints_start, - const std::vector &joints_stop, - moveit::core::RobotModelConstPtr model) +trajectory_msgs::JointTrajectory godel_process_planning::getMoveitPlan( + const std::string& group_name, const std::vector& joints_start, + const std::vector& joints_stop, moveit::core::RobotModelConstPtr model) { const moveit::core::JointModelGroup* group = model->getJointModelGroup(group_name); robot_state::RobotState goal_state(model); @@ -191,7 +193,8 @@ trajectory_msgs::JointTrajectory godel_process_planning::getMoveitPlan(const std // Make connection the planning-service offered by the MoveIt MoveGroup node ros::NodeHandle nh; - ros::ServiceClient client = nh.serviceClient(DEFAULT_MOVEIT_PLANNING_SERVICE_NAME); + ros::ServiceClient client = + nh.serviceClient(DEFAULT_MOVEIT_PLANNING_SERVICE_NAME); trajectory_msgs::JointTrajectory jt; moveit_msgs::GetMotionPlan::Response res; @@ -201,29 +204,31 @@ trajectory_msgs::JointTrajectory godel_process_planning::getMoveitPlan(const std } else { - ROS_ERROR("%s: Unable to call MoveIt path planning service: '%s' or planning failed", __FUNCTION__, DEFAULT_MOVEIT_PLANNING_SERVICE_NAME.c_str()); + ROS_ERROR("%s: Unable to call MoveIt path planning service: '%s' or planning failed", + __FUNCTION__, DEFAULT_MOVEIT_PLANNING_SERVICE_NAME.c_str()); throw std::runtime_error("Unable to generate MoveIt path plan"); } return jt; } -trajectory_msgs::JointTrajectory -godel_process_planning::planFreeMove(descartes_core::RobotModel& model, - const std::string& group_name, - moveit::core::RobotModelConstPtr moveit_model, - const std::vector& start, - const std::vector& stop) +trajectory_msgs::JointTrajectory godel_process_planning::planFreeMove( + descartes_core::RobotModel& model, const std::string& group_name, + moveit::core::RobotModelConstPtr moveit_model, const std::vector& start, + const std::vector& stop) { // Using a mutable model, turns collision checking on for just the // period of this function. Functions called in this function may // throw exceptions and this makes sure the system state is always // valid - struct CollisionsGuard { - CollisionsGuard(descartes_core::RobotModel& model) : model_(model) { + struct CollisionsGuard + { + CollisionsGuard(descartes_core::RobotModel& model) : model_(model) + { model.setCheckCollisions(true); ROS_WARN_STREAM("Enabling collision"); } - ~CollisionsGuard() { + ~CollisionsGuard() + { model_.setCheckCollisions(false); ROS_WARN_STREAM("Disable collision"); } @@ -231,7 +236,7 @@ godel_process_planning::planFreeMove(descartes_core::RobotModel& model, }; // Create gaurd to enable collisions only for this function - CollisionsGuard guard (model); + CollisionsGuard guard(model); // Attempt joint interpolated motion DescartesTraj joint_approach = createJointPath(start, stop); diff --git a/godel_process_planning/src/common_utils.h b/godel_process_planning/src/common_utils.h index 8aed583d..e52f8f66 100644 --- a/godel_process_planning/src/common_utils.h +++ b/godel_process_planning/src/common_utils.h @@ -13,7 +13,7 @@ namespace godel_process_planning { - typedef std::vector DescartesTraj; +typedef std::vector DescartesTraj; /** * @brief Converts a point relative to given pose into a robot tool pose (i.e. flip z) @@ -21,106 +21,107 @@ namespace godel_process_planning * @param pt 3-dimensional offset from pose for the given point * @return Tool pose corresponding to this surface point */ - Eigen::Affine3d createNominalTransform(const geometry_msgs::Pose& ref_pose, - const geometry_msgs::Point& pt); +Eigen::Affine3d createNominalTransform(const geometry_msgs::Pose& ref_pose, + const geometry_msgs::Point& pt); - /** - * @brief Given a path and robot model, this method creates a descartes planner and attempts to solve the path - * @param in_path Trajectory to solve - * @param robot_model Robot model used for IK/FK by planner - * @param out_path The solution path, if found, otherwise undefined - * @return True if path was found and result placed inside 'out_path'; False otherwise. - */ - bool descartesSolve(const DescartesTraj& in_path, - descartes_core::RobotModelConstPtr robot_model, - DescartesTraj& out_path); - /** - * @brief Extracts joint position values from Descartes trajectory and packs them into a ROS message - * @param solution The Descartes trajectory used to generate nominal joint trajectory - * @param model The robot model used in generating the above trajectory - * @return A ROS joint trajectory with only the 'points' field filled in; you must add header/joint name info - */ - trajectory_msgs::JointTrajectory toROSTrajectory(const DescartesTraj& solution, - const descartes_core::RobotModel& model); - /** - * @brief Updates the joint names, frame id, and time stamp of the given trajectory - * @param joints Joint names; listed in same order as the values they correspond to - * @param traj The trajectory to modify - */ - void fillTrajectoryHeaders(const std::vector& joints, - trajectory_msgs::JointTrajectory& traj); - - /** - * @brief Attempts to capture the most recent JointState from the given topic - * @param topic The joint topic name on which to listen for the robot state - * @return The joint positions captured from the topic or a std::runtime_error - */ - std::vector getCurrentJointState(const std::string& topic); +/** + * @brief Given a path and robot model, this method creates a descartes planner and attempts to + * solve the path + * @param in_path Trajectory to solve + * @param robot_model Robot model used for IK/FK by planner + * @param out_path The solution path, if found, otherwise undefined + * @return True if path was found and result placed inside 'out_path'; False otherwise. + */ +bool descartesSolve(const DescartesTraj& in_path, descartes_core::RobotModelConstPtr robot_model, + DescartesTraj& out_path); +/** + * @brief Extracts joint position values from Descartes trajectory and packs them into a ROS message + * @param solution The Descartes trajectory used to generate nominal joint trajectory + * @param model The robot model used in generating the above trajectory + * @return A ROS joint trajectory with only the 'points' field filled in; you must add header/joint + * name info + */ +trajectory_msgs::JointTrajectory toROSTrajectory(const DescartesTraj& solution, + const descartes_core::RobotModel& model); +/** + * @brief Updates the joint names, frame id, and time stamp of the given trajectory + * @param joints Joint names; listed in same order as the values they correspond to + * @param traj The trajectory to modify + */ +void fillTrajectoryHeaders(const std::vector& joints, + trajectory_msgs::JointTrajectory& traj); - /** - * @brief Creates descartes trajectory consisting of cartesian positions in a linear path between start and stop - * @param start The start pose of the linear interpolation - * @param stop The final pose of the linear interpolation - * @param ds The distance (m) between points in the linear path; defaults to 10cm - * @return A linear interpolated path from start to stop in Descartes format - */ - DescartesTraj createLinearPath(const Eigen::Affine3d& start, - const Eigen::Affine3d& stop, - double ds = 0.1); - /** - * @brief Creates descartes trajectory consisting of joint interpolated positions from start to stop - * @param start The initial joint configuration - * @param stop The final joint configuration - * @param dtheta The maximum angle change (radians) for any joint between sequential points - * @return A joint-interpolated path from start to stop in Descartes format - */ - DescartesTraj createJointPath(const std::vector& start, - const std::vector& stop, - double dtheta = M_PI / 180.0); - /** - * @brief Invokes MoveGroup's 'plan_kinematic_path' service to create a collision free path between joints_start and joints_stop - * @brief group_name The name of the MoveIt move-group associated with this plan - * @param joints_start The initial robot configuration - * @param joints_stop The target robot configuration - * @param model The MoveIt model containing the move-group 'group_name' - * @return Joint Trajectory represeting motion from joints_start to joints_stop; or std::runtime_error - */ - trajectory_msgs::JointTrajectory getMoveitPlan(const std::string& group_name, - const std::vector& joints_start, - const std::vector& joints_stop, - moveit::core::RobotModelConstPtr model); - /** - * @brief A helper function to make getting the nominal joint value out of a Descartes point more concise - * @param model The associated Descartes robot model - * @param pt Descartes trajectory point from which to pull the joint values - * @return The joint values from 'pt' obtained as a result of a call to getNominalJointPose() - */ - static inline - std::vector extractJoints(const descartes_core::RobotModel& model, - const descartes_core::TrajectoryPt& pt) - { - std::vector dummy, result; - pt.getNominalJointPose(dummy, model, result); - return result; - } +/** + * @brief Attempts to capture the most recent JointState from the given topic + * @param topic The joint topic name on which to listen for the robot state + * @return The joint positions captured from the topic or a std::runtime_error + */ +std::vector getCurrentJointState(const std::string& topic); - /** - * @brief A planning helper function for getting a valid path between start and stop; first attempts a joint interpolated motion - * to see if its collision free. If not, it invokes MoveIt as a backup. - * @param model Associated descartes robot model - * @param group_name Name of moveit move-group associated with the moveit model - * @param moveit_model the moveit robot description associated with this item - * @param start Initial robot configuration - * @param stop Final robot configuration - * @return A collision-free path from start to stop - */ - trajectory_msgs::JointTrajectory planFreeMove(descartes_core::RobotModel& model, - const std::string &group_name, - moveit::core::RobotModelConstPtr moveit_model, - const std::vector& start, - const std::vector& stop); +/** + * @brief Creates descartes trajectory consisting of cartesian positions in a linear path between + * start and stop + * @param start The start pose of the linear interpolation + * @param stop The final pose of the linear interpolation + * @param ds The distance (m) between points in the linear path; defaults to 10cm + * @return A linear interpolated path from start to stop in Descartes format + */ +DescartesTraj createLinearPath(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, + double ds = 0.1); +/** + * @brief Creates descartes trajectory consisting of joint interpolated positions from start to stop + * @param start The initial joint configuration + * @param stop The final joint configuration + * @param dtheta The maximum angle change (radians) for any joint between sequential points + * @return A joint-interpolated path from start to stop in Descartes format + */ +DescartesTraj createJointPath(const std::vector& start, const std::vector& stop, + double dtheta = M_PI / 180.0); +/** + * @brief Invokes MoveGroup's 'plan_kinematic_path' service to create a collision free path between + * joints_start and joints_stop + * @brief group_name The name of the MoveIt move-group associated with this plan + * @param joints_start The initial robot configuration + * @param joints_stop The target robot configuration + * @param model The MoveIt model containing the move-group 'group_name' + * @return Joint Trajectory represeting motion from joints_start to joints_stop; or + * std::runtime_error + */ +trajectory_msgs::JointTrajectory getMoveitPlan(const std::string& group_name, + const std::vector& joints_start, + const std::vector& joints_stop, + moveit::core::RobotModelConstPtr model); +/** + * @brief A helper function to make getting the nominal joint value out of a Descartes point more + * concise + * @param model The associated Descartes robot model + * @param pt Descartes trajectory point from which to pull the joint values + * @return The joint values from 'pt' obtained as a result of a call to getNominalJointPose() + */ +static inline std::vector extractJoints(const descartes_core::RobotModel& model, + const descartes_core::TrajectoryPt& pt) +{ + std::vector dummy, result; + pt.getNominalJointPose(dummy, model, result); + return result; +} +/** + * @brief A planning helper function for getting a valid path between start and stop; first attempts + * a joint interpolated motion + * to see if its collision free. If not, it invokes MoveIt as a backup. + * @param model Associated descartes robot model + * @param group_name Name of moveit move-group associated with the moveit model + * @param moveit_model the moveit robot description associated with this item + * @param start Initial robot configuration + * @param stop Final robot configuration + * @return A collision-free path from start to stop + */ +trajectory_msgs::JointTrajectory planFreeMove(descartes_core::RobotModel& model, + const std::string& group_name, + moveit::core::RobotModelConstPtr moveit_model, + const std::vector& start, + const std::vector& stop); } #endif // COMMON_UTILS_H - diff --git a/godel_process_planning/src/godel_process_planning.cpp b/godel_process_planning/src/godel_process_planning.cpp index a49672d6..7ac27e81 100644 --- a/godel_process_planning/src/godel_process_planning.cpp +++ b/godel_process_planning/src/godel_process_planning.cpp @@ -1,15 +1,12 @@ #include "godel_process_planning/godel_process_planning.h" #include -godel_process_planning::ProcessPlanningManager::ProcessPlanningManager(const std::string &world_frame, - const std::string &blend_group, - const std::string &blend_tcp, - const std::string &keyence_group, - const std::string &keyence_tcp, - const std::string &robot_model_plugin) - : plugin_loader_("descartes_core", "descartes_core::RobotModel") - , blend_group_name_(blend_group) - , keyence_group_name_(keyence_group) +godel_process_planning::ProcessPlanningManager::ProcessPlanningManager( + const std::string& world_frame, const std::string& blend_group, const std::string& blend_tcp, + const std::string& keyence_group, const std::string& keyence_tcp, + const std::string& robot_model_plugin) + : plugin_loader_("descartes_core", "descartes_core::RobotModel"), + blend_group_name_(blend_group), keyence_group_name_(keyence_group) { // Attempt to load and initialize the blending robot model blend_model_ = plugin_loader_.createInstance(robot_model_plugin); diff --git a/godel_process_planning/src/godel_process_planning_node.cpp b/godel_process_planning/src/godel_process_planning_node.cpp index 3935dd23..a99226bb 100644 --- a/godel_process_planning/src/godel_process_planning_node.cpp +++ b/godel_process_planning/src/godel_process_planning_node.cpp @@ -30,11 +30,15 @@ int main(int argc, char** argv) using godel_process_planning::ProcessPlanningManager; // Creates a planning manager that will create the appropriate planning classes and perform - // all required initialization. It exposes member functions to handle each kind of processing event. - ProcessPlanningManager manager(world_frame, blend_group, blend_tcp, keyence_group, keyence_tcp, robot_model_plugin); + // all required initialization. It exposes member functions to handle each kind of processing + // event. + ProcessPlanningManager manager(world_frame, blend_group, blend_tcp, keyence_group, keyence_tcp, + robot_model_plugin); // Plumb in the appropriate ros services - ros::ServiceServer blend_server = nh.advertiseService(DEFAULT_BLEND_PLANNING_SERVICE, &ProcessPlanningManager::handleBlendPlanning, &manager); - ros::ServiceServer keyence_server = nh.advertiseService(DEFAULT_KEYENCE_PLANNING_SERVICE, &ProcessPlanningManager::handleKeyencePlanning, &manager); + ros::ServiceServer blend_server = nh.advertiseService( + DEFAULT_BLEND_PLANNING_SERVICE, &ProcessPlanningManager::handleBlendPlanning, &manager); + ros::ServiceServer keyence_server = nh.advertiseService( + DEFAULT_KEYENCE_PLANNING_SERVICE, &ProcessPlanningManager::handleKeyencePlanning, &manager); // Serve and wait for shutdown ROS_INFO_STREAM("Godel Process Planning Server Online"); diff --git a/godel_process_planning/src/keyence_process_planning.cpp b/godel_process_planning/src/keyence_process_planning.cpp index 34d6d935..32bc2fda 100644 --- a/godel_process_planning/src/keyence_process_planning.cpp +++ b/godel_process_planning/src/keyence_process_planning.cpp @@ -9,7 +9,8 @@ #include "common_utils.h" // FILE LOCAL CONSTANTS -const static std::string JOINT_TOPIC_NAME = "joint_states"; // ROS topic to subscribe to for robot state +const static std::string JOINT_TOPIC_NAME = + "joint_states"; // ROS topic to subscribe to for robot state namespace godel_process_planning { @@ -25,9 +26,9 @@ static inline descartes_core::TrajectoryPtPtr toDescartesPt(const Eigen::Affine3 { using namespace descartes_trajectory; using namespace descartes_core; - const descartes_core::TimingConstraint tm (dt); + const descartes_core::TimingConstraint tm(dt); - Eigen::Vector3d rpy = pose.rotation().eulerAngles(0,1,2); + Eigen::Vector3d rpy = pose.rotation().eulerAngles(0, 1, 2); Eigen::Vector3d xyz = pose.translation(); double rx = rpy(0), ry = rpy(1), rz = rpy(2); @@ -35,28 +36,31 @@ static inline descartes_core::TrajectoryPtPtr toDescartesPt(const Eigen::Affine3 // By specifying a range of 180 degrees and a step of 180 degrees, the system will sample // at the nominal pose +/- 90 degrees for scanning purposes. - TolerancedFrame frame (pose, - ToleranceBase::zeroTolerance(x,y,z), - ToleranceBase::createSymmetric(rx,ry,rz,0,0,M_PI)); + TolerancedFrame frame( + pose, ToleranceBase::zeroTolerance(x, y, z), + ToleranceBase::createSymmetric(rx, ry, rz, 0, 0, M_PI)); return TrajectoryPtPtr(new CartTrajectoryPt(frame, 0.0, M_PI, tm)); } /** - * @brief transforms an input, in the form of a reference pose and points relative to that pose, into Descartes' + * @brief transforms an input, in the form of a reference pose and points relative to that pose, + * into Descartes' * native format. Also adds in associated parameters. - * @param ref The reference posed that all points are multiplied by. Should be in the world space of the keyence move group. + * @param ref The reference posed that all points are multiplied by. Should be in the world space of + * the keyence move group. * @param points Sequence of points (relative to ref and the world space of blending robot model) * @param params Surface blending parameters, including info such as traversal speed * @return The input trajectory encoded in Descartes points */ -static godel_process_planning::DescartesTraj toDescartesTraj(const geometry_msgs::Pose& ref, - const std::vector& points, - const godel_msgs::ScanPlanParameters& params) +static godel_process_planning::DescartesTraj +toDescartesTraj(const geometry_msgs::Pose& ref, const std::vector& points, + const godel_msgs::ScanPlanParameters& params) { DescartesTraj traj; traj.reserve(points.size()); - if (points.empty()) return traj; + if (points.empty()) + return traj; Eigen::Affine3d last_pose = createNominalTransform(ref, points.front()); @@ -64,7 +68,7 @@ static godel_process_planning::DescartesTraj toDescartesTraj(const geometry_msgs { Eigen::Affine3d this_pose = createNominalTransform(ref, points[i]); double dt = (this_pose.translation() - last_pose.translation()).norm() / params.traverse_spd; - traj.push_back( toDescartesPt(this_pose, dt) ); + traj.push_back(toDescartesPt(this_pose, dt)); last_pose = this_pose; } @@ -78,13 +82,14 @@ static godel_process_planning::DescartesTraj toDescartesTraj(const geometry_msgs * @param res Set of approach, process, and departure trajectories * @return True if a valid plan was generated; false otherwise */ -bool ProcessPlanningManager::handleKeyencePlanning(godel_msgs::KeyenceProcessPlanning::Request& req, - godel_msgs::KeyenceProcessPlanning::Response& res) +bool ProcessPlanningManager::handleKeyencePlanning( + godel_msgs::KeyenceProcessPlanning::Request& req, + godel_msgs::KeyenceProcessPlanning::Response& res) { // Precondition: Input trajectory must be non-zero if (req.path.points.empty()) { - ROS_WARN("%s: Cannot create scan process plan for empty trajectory", __FUNCTION__); + ROS_WARN("%s: Cannot create scan process plan for empty trajectory", __FUNCTION__); return false; } @@ -101,19 +106,23 @@ bool ProcessPlanningManager::handleKeyencePlanning(godel_msgs::KeyenceProcessPla // Calculate nominal pose of initial process point Eigen::Affine3d process_start_pose; - process_points.front()->getNominalCartPose(std::vector(), *keyence_model_, process_start_pose); + process_points.front()->getNominalCartPose(std::vector(), *keyence_model_, + process_start_pose); // Calculate nominal pose of final process point Eigen::Affine3d process_stop_pose; - process_points.back()->getNominalCartPose(std::vector(), *keyence_model_, process_stop_pose); + process_points.back()->getNominalCartPose(std::vector(), *keyence_model_, + process_stop_pose); // Create interpolation segment from init position to process path DescartesTraj to_process = createLinearPath(init_pose, process_start_pose); - to_process.front() = descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); + to_process.front() = + descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); // Create interpolation segment from end of process path to init position DescartesTraj from_process = createLinearPath(process_stop_pose, init_pose); - from_process.back() = descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); + from_process.back() = + descartes_core::TrajectoryPtPtr(new descartes_trajectory::JointTrajectoryPt(current_joints)); // Stitch the above approach and departure trajectories in with the original path DescartesTraj seed_path; @@ -127,20 +136,24 @@ bool ProcessPlanningManager::handleKeyencePlanning(godel_msgs::KeyenceProcessPla return false; } - // Refine the approach and depart plans by attempting joint interpolation and using MoveIt if necessary - trajectory_msgs::JointTrajectory approach = planFreeMove(*keyence_model_, keyence_group_name_, moveit_model_, - extractJoints(*keyence_model_, *solved_path[0]), - extractJoints(*keyence_model_, *solved_path[to_process.size()])); - - trajectory_msgs::JointTrajectory depart = planFreeMove(*keyence_model_, keyence_group_name_, moveit_model_, - extractJoints(*keyence_model_, *solved_path[to_process.size() + process_points.size() -1]), - extractJoints(*keyence_model_, *solved_path[seed_path.size() - 1])); + // Refine the approach and depart plans by attempting joint interpolation and using MoveIt if + // necessary + trajectory_msgs::JointTrajectory approach = + planFreeMove(*keyence_model_, keyence_group_name_, moveit_model_, + extractJoints(*keyence_model_, *solved_path[0]), + extractJoints(*keyence_model_, *solved_path[to_process.size()])); + + trajectory_msgs::JointTrajectory depart = planFreeMove( + *keyence_model_, keyence_group_name_, moveit_model_, + extractJoints(*keyence_model_, *solved_path[to_process.size() + process_points.size() - 1]), + extractJoints(*keyence_model_, *solved_path[seed_path.size() - 1])); // Segment out process path from initial solution - DescartesTraj process_part (solved_path.begin() + to_process.size(), solved_path.end() - from_process.size()); + DescartesTraj process_part(solved_path.begin() + to_process.size(), + solved_path.end() - from_process.size()); trajectory_msgs::JointTrajectory process = toROSTrajectory(process_part, *keyence_model_); // Translate the Descartes trajectory into a ROS joint trajectory - const std::vector< std::string >& joint_names = + const std::vector& joint_names = moveit_model_->getJointModelGroup(keyence_group_name_)->getActiveJointModelNames(); res.plan.trajectory_process = process; @@ -155,5 +168,4 @@ bool ProcessPlanningManager::handleKeyencePlanning(godel_msgs::KeyenceProcessPla return true; } - } diff --git a/godel_process_planning/src/trajectory_utils.cpp b/godel_process_planning/src/trajectory_utils.cpp index c3932326..d92b1689 100644 --- a/godel_process_planning/src/trajectory_utils.cpp +++ b/godel_process_planning/src/trajectory_utils.cpp @@ -1,17 +1,15 @@ #include "trajectory_utils.h" - ////////////////////////////////////////////// // Helper Functions for joint interpolation // ////////////////////////////////////////////// static unsigned int calculateRequiredSteps(const std::vector& start, - const std::vector& stop, - double dtheta) + const std::vector& stop, double dtheta) { // calculate max steps required unsigned steps = 0; - for (std::size_t i = 0; i < start.size() ; ++i) + for (std::size_t i = 0; i < start.size(); ++i) { unsigned this_joint_steps = static_cast(std::abs(start[i] - stop[i]) / dtheta); steps = std::max(steps, this_joint_steps); @@ -21,8 +19,7 @@ static unsigned int calculateRequiredSteps(const std::vector& start, } static std::vector calculateJointSteps(const std::vector& start, - const std::vector& stop, - unsigned int steps) + const std::vector& stop, unsigned int steps) { // Given max steps, calculate delta for each joint std::vector result; @@ -40,19 +37,16 @@ static std::vector interpolateJointSteps(const std::vector& star const std::vector& step_size, unsigned step) { - std::vector result; - result.reserve(start.size()); - for (std::size_t i = 0; i < start.size(); ++i) - result.push_back( start[i] + step_size[i] * step ); - return result; + std::vector result; + result.reserve(start.size()); + for (std::size_t i = 0; i < start.size(); ++i) + result.push_back(start[i] + step_size[i] * step); + return result; } - - godel_process_planning::PoseVector -godel_process_planning::interpolateCartesian(const Eigen::Affine3d &start, - const Eigen::Affine3d &stop, - double ds) +godel_process_planning::interpolateCartesian(const Eigen::Affine3d& start, + const Eigen::Affine3d& stop, double ds) { // Required position change Eigen::Vector3d delta = (stop.translation() - start.translation()); @@ -65,8 +59,8 @@ godel_process_planning::interpolateCartesian(const Eigen::Affine3d &start, Eigen::Vector3d step = delta / steps; // Orientation interpolation - Eigen::Quaterniond start_q (start.rotation()); - Eigen::Quaterniond stop_q (stop.rotation()); + Eigen::Quaterniond start_q(start.rotation()); + Eigen::Quaterniond stop_q(stop.rotation()); double slerp_ratio = 1.0 / steps; godel_process_planning::PoseVector result; @@ -75,17 +69,15 @@ godel_process_planning::interpolateCartesian(const Eigen::Affine3d &start, { Eigen::Vector3d trans = start_pos + step * i; Eigen::Quaterniond q = start_q.slerp(slerp_ratio * i, stop_q); - Eigen::Affine3d pose (Eigen::Translation3d(trans) * q); + Eigen::Affine3d pose(Eigen::Translation3d(trans) * q); result.push_back(pose); } return result; } - godel_process_planning::JointVector -godel_process_planning::interpolateJoint(const std::vector &start, - const std::vector &stop, - double dtheta) +godel_process_planning::interpolateJoint(const std::vector& start, + const std::vector& stop, double dtheta) { godel_process_planning::JointVector result; // joint delta @@ -94,8 +86,8 @@ godel_process_planning::interpolateJoint(const std::vector &start, // Walk interpolation for (std::size_t i = 0; i <= steps; ++i) { - std::vector pos = interpolateJointSteps(start, delta, i); - result.push_back(pos); + std::vector pos = interpolateJointSteps(start, delta, i); + result.push_back(pos); } return result; } diff --git a/godel_process_planning/src/trajectory_utils.h b/godel_process_planning/src/trajectory_utils.h index fce480e6..0eb8d6ac 100644 --- a/godel_process_planning/src/trajectory_utils.h +++ b/godel_process_planning/src/trajectory_utils.h @@ -5,8 +5,8 @@ namespace godel_process_planning { - // Cartesian Interpolation - typedef std::vector > PoseVector; +// Cartesian Interpolation +typedef std::vector > PoseVector; /** * @brief Creates a vector of poses representing linear spatial and rotational interpolation @@ -16,23 +16,21 @@ namespace godel_process_planning * @param ds The cartesian distance (m) between intermediate points * @return Sequence of poses */ - PoseVector - interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, double ds); +PoseVector interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, + double ds); - // Joint Interpolation - typedef std::vector > JointVector; - - /** - * @brief Creates a vector of joint poses linearly interpolating from start to stop - * @param start Initial joint configuration - * @param stop Final joint configuration - * @param dtheta Maximum joint step (radians) between intermediate points - * @return - */ - JointVector - interpolateJoint(const std::vector& start, const std::vector& stop, - double dtheta); +// Joint Interpolation +typedef std::vector > JointVector; +/** + * @brief Creates a vector of joint poses linearly interpolating from start to stop + * @param start Initial joint configuration + * @param stop Final joint configuration + * @param dtheta Maximum joint step (radians) between intermediate points + * @return + */ +JointVector interpolateJoint(const std::vector& start, const std::vector& stop, + double dtheta); } #endif diff --git a/godel_robots/abb/abb_file_suite/include/abb_file_suite/abb_motion_ftp_downloader.h b/godel_robots/abb/abb_file_suite/include/abb_file_suite/abb_motion_ftp_downloader.h index b3729cac..129d4554 100644 --- a/godel_robots/abb/abb_file_suite/include/abb_file_suite/abb_motion_ftp_downloader.h +++ b/godel_robots/abb/abb_file_suite/include/abb_file_suite/abb_motion_ftp_downloader.h @@ -17,10 +17,8 @@ namespace abb_file_suite class AbbMotionFtpDownloader { public: - AbbMotionFtpDownloader(const std::string& ip, - const std::string& listen_topic, - ros::NodeHandle& nh, - bool j23_coupled = false, + AbbMotionFtpDownloader(const std::string& ip, const std::string& listen_topic, + ros::NodeHandle& nh, bool j23_coupled = false, const std::string& temp_file_loc = std::string("/tmp")); /** @@ -36,7 +34,7 @@ class AbbMotionFtpDownloader * @param req Contains the ABSOLUTE path the RAPID file to upload * @param res No fields in the return value * @return True if the FTP transfer was completed; note this doesn't mean the robot - * succeeded or even read the complete file. + * succeeded or even read the complete file. */ bool handleServiceCall(abb_file_suite::ExecuteProgram::Request& req, abb_file_suite::ExecuteProgram::Response& res); @@ -48,7 +46,6 @@ class AbbMotionFtpDownloader const std::string temp_file_loc_; bool j23_coupled_; /** joints 2 and 3 are coupled (as in ABB IRB2400) */ }; - } #endif diff --git a/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_data_structures.h b/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_data_structures.h index ef4db9d2..57b65ca3 100644 --- a/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_data_structures.h +++ b/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_data_structures.h @@ -16,12 +16,12 @@ struct TrajectoryPt typedef std::vector value_type; TrajectoryPt(const std::vector& positions, double duration = 0.0) - : positions_(positions) - , duration_(duration) - {} + : positions_(positions), duration_(duration) + { + } value_type positions_; // degrees - double duration_; // seconds + double duration_; // seconds }; /** @@ -32,15 +32,13 @@ struct TrajectoryPt */ struct ProcessParams { - double spindle_speed; // Tool rotation speed - double tcp_speed; // Tool linear traversal speed - double force; // For non-wolfware software, a general identification of force + double spindle_speed; // Tool rotation speed + double tcp_speed; // Tool linear traversal speed + double force; // For non-wolfware software, a general identification of force std::string output_name; // The I/O that toggles the tool power; We assume 0 is off and 1 is on. - bool wolf_mode; // In the case of Wolfware software, we emit special instructions. - double slide_force; // For WolfWare, a meaure of the cross-slide force (?) + bool wolf_mode; // In the case of Wolfware software, we emit special instructions. + double slide_force; // For WolfWare, a meaure of the cross-slide force (?) }; - } #endif // RAPID_DATA_STRUCTURES_H - diff --git a/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_emitter.h b/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_emitter.h index 27d54976..546afd67 100644 --- a/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_emitter.h +++ b/godel_robots/abb/abb_file_suite/rapid_generator/include/rapid_generator/rapid_emitter.h @@ -6,48 +6,48 @@ #include #include - namespace rapid_emitter { - /** - * @brief Writes a RAPID program to 'os' that moves through a sequence of joint positions - * and toggles an I/O on and off at specific sequence points. - * @param os The output stream; will usually be a std::ofstream - * @param points Sequence of joint-positions and durations - * @param startProcessMotion The first joint-position inside the 'process' segment of the points array - * @param endProcessMotion The final joint-position inside the 'process' segment of the points array - * @param params Process parameters such as tool velocity - * @return Success if the file was successfully generated - */ - bool emitRapidFile(std::ostream& os, - const std::vector& points, - size_t startProcessMotion, - size_t endProcessMotion, - const ProcessParams& params); - - /** - * @brief Writes a RAPID program to 'os' that merely moves through a sequence of points. - * @param os The output stream; will usually be std::ofstream - * @param points Sequence of joint positions and durations to move through - * @param params These parameters are used to provide default motion speeds if duration is left emtpy - * @return True is the file was successfully generated - */ - bool emitJointTrajectoryFile(std::ostream& os, - const std::vector& points, - const ProcessParams& params); - - /** Helper Functions **/ - - // Writes a joint target with id 'n' to the pre-amble section of a custom module - bool emitJointPosition(std::ostream& os, const TrajectoryPt& pt, size_t n); - // Writes a linear motion to joint target 'n'; the final parameters control special parameters for Wolf systems - bool emitGrindMotion(std::ostream& os, const ProcessParams& params, size_t n, bool start = false, bool end = false); - // Writes a free joint move to target 'n' - bool emitFreeMotion(std::ostream& os, const ProcessParams& params, size_t n, double duration, bool stop_at); - // Writes an I/O using the name from 'params' - bool emitSetOutput(std::ostream& os, const ProcessParams& params, size_t value); - // Writes the necessary info for process parameter blocks - bool emitProcessDeclarations(std::ostream& os, const ProcessParams& params, size_t value); +/** + * @brief Writes a RAPID program to 'os' that moves through a sequence of joint positions + * and toggles an I/O on and off at specific sequence points. + * @param os The output stream; will usually be a std::ofstream + * @param points Sequence of joint-positions and durations + * @param startProcessMotion The first joint-position inside the 'process' segment of the points + * array + * @param endProcessMotion The final joint-position inside the 'process' segment of the points array + * @param params Process parameters such as tool velocity + * @return Success if the file was successfully generated + */ +bool emitRapidFile(std::ostream& os, const std::vector& points, + size_t startProcessMotion, size_t endProcessMotion, const ProcessParams& params); + +/** + * @brief Writes a RAPID program to 'os' that merely moves through a sequence of points. + * @param os The output stream; will usually be std::ofstream + * @param points Sequence of joint positions and durations to move through + * @param params These parameters are used to provide default motion speeds if duration is left + * emtpy + * @return True is the file was successfully generated + */ +bool emitJointTrajectoryFile(std::ostream& os, const std::vector& points, + const ProcessParams& params); + +/** Helper Functions **/ + +// Writes a joint target with id 'n' to the pre-amble section of a custom module +bool emitJointPosition(std::ostream& os, const TrajectoryPt& pt, size_t n); +// Writes a linear motion to joint target 'n'; the final parameters control special parameters for +// Wolf systems +bool emitGrindMotion(std::ostream& os, const ProcessParams& params, size_t n, bool start = false, + bool end = false); +// Writes a free joint move to target 'n' +bool emitFreeMotion(std::ostream& os, const ProcessParams& params, size_t n, double duration, + bool stop_at); +// Writes an I/O using the name from 'params' +bool emitSetOutput(std::ostream& os, const ProcessParams& params, size_t value); +// Writes the necessary info for process parameter blocks +bool emitProcessDeclarations(std::ostream& os, const ProcessParams& params, size_t value); } #endif diff --git a/godel_robots/abb/abb_file_suite/rapid_generator/src/rapid_emitter.cpp b/godel_robots/abb/abb_file_suite/rapid_generator/src/rapid_emitter.cpp index 182e6272..069846fb 100644 --- a/godel_robots/abb/abb_file_suite/rapid_generator/src/rapid_emitter.cpp +++ b/godel_robots/abb/abb_file_suite/rapid_generator/src/rapid_emitter.cpp @@ -2,10 +2,8 @@ #include -bool rapid_emitter::emitRapidFile(std::ostream& os, - const std::vector& points, - size_t startProcessMotion, - size_t endProcessMotion, +bool rapid_emitter::emitRapidFile(std::ostream& os, const std::vector& points, + size_t startProcessMotion, size_t endProcessMotion, const ProcessParams& params) { // Write header @@ -23,14 +21,17 @@ bool rapid_emitter::emitRapidFile(std::ostream& os, // For 0 to lengthFreeMotion, emit free moves for (std::size_t i = 0; i < startProcessMotion; ++i) { - if (i == 0) emitFreeMotion(os, params, i, 0.0, true); - else if (i == (startProcessMotion - 1)) emitFreeMotion(os, params, i, points[i].duration_, true); - else emitFreeMotion(os, params, i, points[i].duration_, false); + if (i == 0) + emitFreeMotion(os, params, i, 0.0, true); + else if (i == (startProcessMotion - 1)) + emitFreeMotion(os, params, i, points[i].duration_, true); + else + emitFreeMotion(os, params, i, points[i].duration_, false); } - + // Turn on the tool emitSetOutput(os, params, 1); - + // for lengthFreeMotion to end of points, emit grind moves for (std::size_t i = startProcessMotion; i < endProcessMotion; ++i) { @@ -38,7 +39,7 @@ bool rapid_emitter::emitRapidFile(std::ostream& os, { emitGrindMotion(os, params, i, true, false); } - else if (i == endProcessMotion-1) + else if (i == endProcessMotion - 1) { emitGrindMotion(os, params, i, false, true); } @@ -47,16 +48,19 @@ bool rapid_emitter::emitRapidFile(std::ostream& os, emitGrindMotion(os, params, i); } } - + // Turn off the tool emitSetOutput(os, params, 0); - + // for lengthFreeMotion to end of points, emit grind moves for (std::size_t i = endProcessMotion; i < points.size(); ++i) { - if (i == endProcessMotion) emitFreeMotion(os, params, i, 0.0, true); - else if (i == (points.size() - 1)) emitFreeMotion(os, params, i, points[i].duration_, true); - else emitFreeMotion(os, params, i, points[i].duration_, false); + if (i == endProcessMotion) + emitFreeMotion(os, params, i, 0.0, true); + else if (i == (points.size() - 1)) + emitFreeMotion(os, params, i, points[i].duration_, true); + else + emitFreeMotion(os, params, i, points[i].duration_, false); } os << "EndProc\n"; @@ -66,8 +70,8 @@ bool rapid_emitter::emitRapidFile(std::ostream& os, return true; } - -bool rapid_emitter::emitGrindMotion(std::ostream& os, const ProcessParams& params, size_t n, bool start, bool end) +bool rapid_emitter::emitGrindMotion(std::ostream& os, const ProcessParams& params, size_t n, + bool start, bool end) { // The 'fine' zoning means the robot will stop at this point @@ -96,9 +100,11 @@ bool rapid_emitter::emitGrindMotion(std::ostream& os, const ProcessParams& param return os.good(); } -bool rapid_emitter::emitFreeMotion(std::ostream& os, const ProcessParams& params, size_t n, double duration, bool stop_at) +bool rapid_emitter::emitFreeMotion(std::ostream& os, const ProcessParams& params, size_t n, + double duration, bool stop_at) { - // We want the robot to move smoothly and stop at the last point; these tolerances ensure that this happens + // We want the robot to move smoothly and stop at the last point; these tolerances ensure that + // this happens const char* zone = stop_at ? "fine" : "z20"; if (duration <= 0.0) @@ -106,22 +112,23 @@ bool rapid_emitter::emitFreeMotion(std::ostream& os, const ProcessParams& params os << "MoveJ CalcRobT(jTarget_" << n << ",tool0), vMotionSpeed," << zone << ", tool0;\n"; } else - { - os << "MoveJ CalcRobT(jTarget_" << n << ",tool0), vMotionSpeed, \\T:=" << duration << ", " << zone << ", tool0;\n"; + { + os << "MoveJ CalcRobT(jTarget_" << n << ",tool0), vMotionSpeed, \\T:=" << duration << ", " + << zone << ", tool0;\n"; } return os.good(); } bool rapid_emitter::emitJointPosition(std::ostream& os, const TrajectoryPt& pt, size_t n) { os << "TASK PERS jointtarget jTarget_" << n << ":=[["; - for (size_t i = 0; i < pt.positions_.size() ; i++) + for (size_t i = 0; i < pt.positions_.size(); i++) + { + os << pt.positions_[i]; + if (i < pt.positions_.size() - 1) { - os << pt.positions_[i]; - if (i < pt.positions_.size()-1) - { - os << ","; - } + os << ","; } + } // We assume a six axis robot here. os << "],[9E9,9E9,9E9,9E9,9E9,9E9]];\n"; return true; @@ -135,28 +142,36 @@ bool rapid_emitter::emitSetOutput(std::ostream& os, const ProcessParams& params, // This wait time is inserted here to ensure that the zone is achieved BEFORE the I/O happens os << "WaitTime\\InPos, 0.01;\n"; os << "SETDO " << params.output_name << ", " << value << ";\n"; - } + } return os.good(); -} +} -bool rapid_emitter::emitProcessDeclarations(std::ostream& os, const ProcessParams& params, size_t value) +bool rapid_emitter::emitProcessDeclarations(std::ostream& os, const ProcessParams& params, + size_t value) { if (params.wolf_mode) { - os << "TASK PERS grinddata gr1:=[" << params.tcp_speed << "," << params.spindle_speed << "," << params.slide_force << ",FALSE,FALSE,FALSE,0,0];\n"; + os << "TASK PERS grinddata gr1:=[" << params.tcp_speed << "," << params.spindle_speed << "," + << params.slide_force << ",FALSE,FALSE,FALSE,0,0];\n"; } else { // Process Speed - os << "CONST speeddata vProcessSpeed:=[" << params.tcp_speed << "," << params.tcp_speed << ",50,50];\n"; + os << "CONST speeddata vProcessSpeed:=[" << params.tcp_speed << "," << params.tcp_speed + << ",50,50];\n"; } // Free motion speed - os << "CONST speeddata vMotionSpeed:=[" << "200" << "," << "30" << ",500,500];\n"; + os << "CONST speeddata vMotionSpeed:=[" + << "200" + << "," + << "30" + << ",500,500];\n"; return os.good(); } - -bool rapid_emitter::emitJointTrajectoryFile(std::ostream &os, const std::vector &points, const ProcessParams ¶ms) +bool rapid_emitter::emitJointTrajectoryFile(std::ostream& os, + const std::vector& points, + const ProcessParams& params) { // Write header os << "MODULE mGodel_Blend\n\n"; @@ -178,7 +193,7 @@ bool rapid_emitter::emitJointTrajectoryFile(std::ostream &os, const std::vector< // Write first point as normal move abs j so that the robot gets to where it needs to be emitFreeMotion(os, params, 0, 0.0, true); - // The second motion + // The second motion for (std::size_t i = 1; i < points.size() - 1; ++i) { emitFreeMotion(os, params, i, points[i].duration_, false); diff --git a/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader.cpp b/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader.cpp index 32303d4a..ab8413a7 100644 --- a/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader.cpp +++ b/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader.cpp @@ -12,64 +12,56 @@ const static std::string EXECUTE_PROGRAM_SERVICE_NAME = "execute_program"; const static std::string RAPID_MODULE_NAME = "mGodel_blend.mod"; // Utility functions -static double toDegrees(const double radians) -{ - return radians * 180.0 / M_PI; -} +static double toDegrees(const double radians) { return radians * 180.0 / M_PI; } static std::vector toDegrees(const std::vector& radians) { std::vector result; result.reserve(radians.size()); - for (std::size_t i = 0; i < radians.size(); ++i) result.push_back(toDegrees(radians[i])); + for (std::size_t i = 0; i < radians.size(); ++i) + result.push_back(toDegrees(radians[i])); return result; } -static void linkageAdjust(std::vector& joints) -{ - joints[2] += joints[1]; -} +static void linkageAdjust(std::vector& joints) { joints[2] += joints[1]; } /** * Joins a directory and filename while being independent to whether the user * added a trailing slash or not. - * + * * @param dir absolute path to directory - * @param filename file name + * @param filename file name * @return absolute path to file */ static std::string fileJoin(const std::string& dir, const std::string& filename) { - if (dir[dir.length() - 1] == '/') return (dir + filename); - else return (dir + std::string("/") + filename); + if (dir[dir.length() - 1] == '/') + return (dir + filename); + else + return (dir + std::string("/") + filename); } -abb_file_suite::AbbMotionFtpDownloader::AbbMotionFtpDownloader(const std::string &ip, - const std::string &listen_topic, - ros::NodeHandle &nh, +abb_file_suite::AbbMotionFtpDownloader::AbbMotionFtpDownloader(const std::string& ip, + const std::string& listen_topic, + ros::NodeHandle& nh, bool j23_coupled, const std::string& temp_file_loc) - : ip_(ip) - , temp_file_loc_(temp_file_loc) - , j23_coupled_(j23_coupled) + : ip_(ip), temp_file_loc_(temp_file_loc), j23_coupled_(j23_coupled) { - trajectory_sub_ = nh.subscribe( - listen_topic, - 10, - &AbbMotionFtpDownloader::handleJointTrajectory, - this); - - server_ = nh.advertiseService(EXECUTE_PROGRAM_SERVICE_NAME, - &AbbMotionFtpDownloader::handleServiceCall, - this); + trajectory_sub_ = + nh.subscribe(listen_topic, 10, &AbbMotionFtpDownloader::handleJointTrajectory, this); + + server_ = nh.advertiseService(EXECUTE_PROGRAM_SERVICE_NAME, + &AbbMotionFtpDownloader::handleServiceCall, this); } -void abb_file_suite::AbbMotionFtpDownloader::handleJointTrajectory(const trajectory_msgs::JointTrajectory &traj) +void abb_file_suite::AbbMotionFtpDownloader::handleJointTrajectory( + const trajectory_msgs::JointTrajectory& traj) { // Create temporary file const std::string temp_file_path = fileJoin(temp_file_loc_, RAPID_MODULE_NAME); // generate temporary file with appropriate rapid code - std::ofstream ofh (temp_file_path.c_str()); + std::ofstream ofh(temp_file_path.c_str()); if (!ofh) { @@ -82,16 +74,17 @@ void abb_file_suite::AbbMotionFtpDownloader::handleJointTrajectory(const traject for (std::size_t i = 0; i < traj.points.size(); ++i) { std::vector tmp = toDegrees(traj.points[i].positions); - if (j23_coupled_) linkageAdjust(tmp); + if (j23_coupled_) + linkageAdjust(tmp); double duration = 0.0; // Timing if (i > 0) { - duration = (traj.points[i].time_from_start - traj.points[i-1].time_from_start).toSec(); + duration = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).toSec(); } - rapid_emitter::TrajectoryPt pt (tmp, duration); + rapid_emitter::TrajectoryPt pt(tmp, duration); pts.push_back(pt); } @@ -108,11 +101,11 @@ void abb_file_suite::AbbMotionFtpDownloader::handleJointTrajectory(const traject } } -bool abb_file_suite::AbbMotionFtpDownloader::handleServiceCall(abb_file_suite::ExecuteProgram::Request &req, - abb_file_suite::ExecuteProgram::Response &res) +bool abb_file_suite::AbbMotionFtpDownloader::handleServiceCall( + abb_file_suite::ExecuteProgram::Request& req, abb_file_suite::ExecuteProgram::Response& res) { // Check for existence - std::ifstream ifh (req.file_path.c_str()); + std::ifstream ifh(req.file_path.c_str()); if (!ifh) { ROS_WARN("Could not open file '%s'.", req.file_path.c_str()); diff --git a/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader_node.cpp b/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader_node.cpp index d2e15dd4..cbb1ef07 100644 --- a/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader_node.cpp +++ b/godel_robots/abb/abb_file_suite/src/abb_motion_ftp_downloader_node.cpp @@ -22,7 +22,8 @@ int main(int argc, char** argv) abb_file_suite::AbbMotionFtpDownloader ftp(ip, topic, nh, j23_coupled); - ROS_INFO("ABB FTP-Trajectory-Downloader (ip %s) initialized. Listening on topic %s.", ip.c_str(), topic.c_str()); + ROS_INFO("ABB FTP-Trajectory-Downloader (ip %s) initialized. Listening on topic %s.", ip.c_str(), + topic.c_str()); ros::spin(); return 0; diff --git a/godel_robots/abb/abb_file_suite/src/ftp_upload.cpp b/godel_robots/abb/abb_file_suite/src/ftp_upload.cpp index bc2bd6e3..12a1694e 100644 --- a/godel_robots/abb/abb_file_suite/src/ftp_upload.cpp +++ b/godel_robots/abb/abb_file_suite/src/ftp_upload.cpp @@ -14,7 +14,7 @@ const static long DEFAULT_TIMEOUT = 0; // Don't timeout const static long DEFAULT_RETRIES = 5; /* parse headers for Content-Length */ -static size_t getcontentlengthfunc(void *ptr, size_t size, size_t nmemb, void *stream) +static size_t getcontentlengthfunc(void* ptr, size_t size, size_t nmemb, void* stream) { int r; long len = 0; @@ -23,21 +23,21 @@ static size_t getcontentlengthfunc(void *ptr, size_t size, size_t nmemb, void *s r = sscanf(static_cast(ptr), "Content-Length: %ld\n", &len); if (r) /* Microsoft: we don't read the specs */ - *((long *) stream) = len; + *((long*)stream) = len; return size * nmemb; } /* discard downloaded data */ -static size_t discardfunc(void *ptr, size_t size, size_t nmemb, void *stream) +static size_t discardfunc(void* ptr, size_t size, size_t nmemb, void* stream) { return size * nmemb; } /* read data to upload */ -static size_t readfunc(void *ptr, size_t size, size_t nmemb, void *stream) +static size_t readfunc(void* ptr, size_t size, size_t nmemb, void* stream) { - FILE *f = static_cast(stream); + FILE* f = static_cast(stream); size_t n; if (ferror(f)) @@ -48,17 +48,17 @@ static size_t readfunc(void *ptr, size_t size, size_t nmemb, void *stream) return n; } - -static int upload(CURL *curlhandle, const char * remotepath, const char * localpath, - long timeout, long tries) +static int upload(CURL* curlhandle, const char* remotepath, const char* localpath, long timeout, + long tries) { - FILE *f; + FILE* f; long uploaded_len = 0; CURLcode r = CURLE_GOT_NOTHING; int c; f = fopen(localpath, "rb"); - if (f == NULL) { + if (f == NULL) + { perror(NULL); return 0; } @@ -85,9 +85,11 @@ static int upload(CURL *curlhandle, const char * remotepath, const char * localp curl_easy_setopt(curlhandle, CURLOPT_VERBOSE, 1L); - for (c = 0; (r != CURLE_OK) && (c < tries); c++) { + for (c = 0; (r != CURLE_OK) && (c < tries); c++) + { /* are we resuming? */ - if (c) { /* yes */ + if (c) + { /* yes */ /* determine the length of the file already written */ /* @@ -112,7 +114,8 @@ static int upload(CURL *curlhandle, const char * remotepath, const char * localp curl_easy_setopt(curlhandle, CURLOPT_APPEND, 1L); } - else { /* no */ + else + { /* no */ curl_easy_setopt(curlhandle, CURLOPT_APPEND, 0L); } @@ -123,15 +126,16 @@ static int upload(CURL *curlhandle, const char * remotepath, const char * localp if (r == CURLE_OK) return 1; - else { + else + { fprintf(stderr, "%s\n", curl_easy_strerror(r)); return 0; } } -bool abb_file_suite::uploadFile(const std::string &ftp_addr, const std::string &filepath) +bool abb_file_suite::uploadFile(const std::string& ftp_addr, const std::string& filepath) { - CURL *curlhandle = NULL; + CURL* curlhandle = NULL; curl_global_init(CURL_GLOBAL_ALL); curlhandle = curl_easy_init(); diff --git a/godel_robots/abb/abb_file_suite/src/ftp_upload.h b/godel_robots/abb/abb_file_suite/src/ftp_upload.h index f05c0593..d9b14dc4 100644 --- a/godel_robots/abb/abb_file_suite/src/ftp_upload.h +++ b/godel_robots/abb/abb_file_suite/src/ftp_upload.h @@ -7,9 +7,6 @@ namespace abb_file_suite { bool uploadFile(const std::string& ftp_addr, const std::string& filepath); - } - #endif // FTP_UPLOAD_H - diff --git a/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/include/abb_irb2400_descartes/abb_irb2400_robot_model.h b/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/include/abb_irb2400_descartes/abb_irb2400_robot_model.h index 94b1a4d8..339b2ab5 100644 --- a/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/include/abb_irb2400_descartes/abb_irb2400_robot_model.h +++ b/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/include/abb_irb2400_descartes/abb_irb2400_robot_model.h @@ -22,32 +22,30 @@ namespace abb_irb2400_descartes { - class AbbIrb2400RobotModel: public descartes_moveit::MoveitStateAdapter, - public irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin +class AbbIrb2400RobotModel : public descartes_moveit::MoveitStateAdapter, + public irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin { - public: - - AbbIrb2400RobotModel(); - - virtual bool initialize(const std::string& robot_description, const std::string& group_name, - const std::string& world_frame,const std::string& tcp_frame); - - virtual bool getAllIK(const Eigen::Affine3d &pose, std::vector > &joint_poses) const; - - virtual descartes_core::RobotModelPtr clone() const - { - descartes_core::RobotModelPtr ptr (new AbbIrb2400RobotModel()); - ptr->initialize("robot_description", descartes_moveit::MoveitStateAdapter::group_name_, world_frame_, tool_frame_); - return ptr; - } - - protected: - - descartes_core::Frame world_to_base_;// world to arm base - descartes_core::Frame tool_to_tip_; // from urdf tool to arm tool - +public: + AbbIrb2400RobotModel(); + + virtual bool initialize(const std::string& robot_description, const std::string& group_name, + const std::string& world_frame, const std::string& tcp_frame); + + virtual bool getAllIK(const Eigen::Affine3d& pose, + std::vector >& joint_poses) const; + + virtual descartes_core::RobotModelPtr clone() const + { + descartes_core::RobotModelPtr ptr(new AbbIrb2400RobotModel()); + ptr->initialize("robot_description", descartes_moveit::MoveitStateAdapter::group_name_, + world_frame_, tool_frame_); + return ptr; + } + +protected: + descartes_core::Frame world_to_base_; // world to arm base + descartes_core::Frame tool_to_tip_; // from urdf tool to arm tool }; - } -#endif //MOTOMAN_SIA20D_ROBOT_MODEL +#endif // MOTOMAN_SIA20D_ROBOT_MODEL diff --git a/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp b/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp index 072472c9..ec661e86 100644 --- a/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp +++ b/godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp @@ -27,102 +27,101 @@ using namespace irb2400_ikfast_manipulator_plugin; namespace abb_irb2400_descartes { - AbbIrb2400RobotModel::AbbIrb2400RobotModel(): - world_to_base_(Eigen::Affine3d::Identity()), - tool_to_tip_(Eigen::Affine3d::Identity()) - { +AbbIrb2400RobotModel::AbbIrb2400RobotModel() + : world_to_base_(Eigen::Affine3d::Identity()), tool_to_tip_(Eigen::Affine3d::Identity()) +{ +} - } +bool AbbIrb2400RobotModel::initialize(const std::string& robot_description, + const std::string& group_name, const std::string& world_frame, + const std::string& tcp_frame) +{ + MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame); + irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin::initialize( + robot_description, group_name, MOTOMAN_SIA20D_BASE_LINK, MOTOMAN_SIA20D_TOOL_LINK, 0.001); - bool AbbIrb2400RobotModel::initialize(const std::string& robot_description, const std::string& group_name, - const std::string& world_frame,const std::string& tcp_frame) + // initialize world transformations + if (tcp_frame != getTipFrame()) { - MoveitStateAdapter::initialize(robot_description,group_name,world_frame,tcp_frame); - irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin::initialize(robot_description, group_name, MOTOMAN_SIA20D_BASE_LINK, - MOTOMAN_SIA20D_TOOL_LINK, 0.001); - - // initialize world transformations - if(tcp_frame != getTipFrame()) - { - tool_to_tip_ = descartes_core::Frame(robot_state_->getFrameTransform(tcp_frame).inverse()* - robot_state_->getFrameTransform(getTipFrame())); - } - - if(world_frame != getBaseFrame()) - { - world_to_base_ = descartes_core::Frame(world_to_root_.frame * robot_state_->getFrameTransform(getBaseFrame())); - } - - return true; - + tool_to_tip_ = descartes_core::Frame(robot_state_->getFrameTransform(tcp_frame).inverse() * + robot_state_->getFrameTransform(getTipFrame())); } - bool AbbIrb2400RobotModel::getAllIK(const Eigen::Affine3d &pose, - std::vector< std::vector > &joint_poses) const + if (world_frame != getBaseFrame()) { - bool rtn = false; + world_to_base_ = descartes_core::Frame(world_to_root_.frame * + robot_state_->getFrameTransform(getBaseFrame())); + } - std::vector vfree(free_params_.size()); - KDL::Frame frame; - Eigen::Affine3d tool_pose = world_to_base_.frame_inv* pose* tool_to_tip_.frame; - tf::transformEigenToKDL(tool_pose, frame); + return true; +} - ikfast::IkSolutionList solutions; +bool AbbIrb2400RobotModel::getAllIK(const Eigen::Affine3d& pose, + std::vector >& joint_poses) const +{ + bool rtn = false; - ROS_DEBUG_STREAM("Number of free params: " << vfree.size()); + std::vector vfree(free_params_.size()); + KDL::Frame frame; + Eigen::Affine3d tool_pose = world_to_base_.frame_inv * pose * tool_to_tip_.frame; + tf::transformEigenToKDL(tool_pose, frame); - int numsol = solve(frame, vfree, solutions); + ikfast::IkSolutionList solutions; - ROS_DEBUG_STREAM("Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM("Number of free params: " << vfree.size()); - joint_poses.clear(); + int numsol = solve(frame, vfree, solutions); - if(numsol) - { - for(int s = 0; s < numsol; ++s) - { - std::vector sol; - getSolution(solutions,s,sol); - ROS_DEBUG_STREAM("Original solution size: " << sol.size() - <<", number of joints: " << num_joints_); + ROS_DEBUG_STREAM("Found " << numsol << " solutions from IKFast"); - 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]-JOINT_LIMIT_TOLERANCE)) || - (sol[i] > (joint_max_vector_[i]+JOINT_LIMIT_TOLERANCE)) ) ) - { - // One element of solution is not within limits - obeys_limits = false; - ROS_DEBUG_STREAM("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; - } - } + joint_poses.clear(); - if(obeys_limits) + if (numsol) + { + for (int s = 0; s < numsol; ++s) + { + std::vector sol; + getSolution(solutions, s, sol); + ROS_DEBUG_STREAM("Original solution size: " << sol.size() + << ", number of joints: " << num_joints_); + + 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] - JOINT_LIMIT_TOLERANCE)) || + (sol[i] > (joint_max_vector_[i] + JOINT_LIMIT_TOLERANCE)))) { - joint_poses.push_back(sol); + // One element of solution is not within limits + obeys_limits = false; + ROS_DEBUG_STREAM("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(joint_poses.empty()) - { - ROS_DEBUG_STREAM("GetAllIK has not solutions"); - rtn = false; - } - else - { - rtn = true; + if (obeys_limits) + { + joint_poses.push_back(sol); + } } + } - return rtn; + if (joint_poses.empty()) + { + ROS_DEBUG_STREAM("GetAllIK has not solutions"); + rtn = false; + } + else + { + rtn = true; } + return rtn; +} } - -PLUGINLIB_EXPORT_CLASS(abb_irb2400_descartes::AbbIrb2400RobotModel,descartes_core::RobotModel) +PLUGINLIB_EXPORT_CLASS(abb_irb2400_descartes::AbbIrb2400RobotModel, descartes_core::RobotModel) diff --git a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_moveit_plugin.hpp b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_moveit_plugin.hpp index 923832e7..407c8502 100644 --- a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_moveit_plugin.hpp +++ b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_moveit_plugin.hpp @@ -2,7 +2,8 @@ * * Software License Agreement (BSD License) * - * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer IPA + * 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 @@ -53,13 +54,17 @@ // 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 }; +enum SEARCH_MODE +{ + OPTIMIZE_FREE_JOINT = 1, + OPTIMIZE_MAX_JOINT = 2 +}; namespace irb2400_ikfast_manipulator_plugin { const static std::string ABB_BASE_LINK = "base_link"; -const static std::string ABB_TIP_LINK ="tool0"; +const static std::string ABB_TIP_LINK = "tool0"; #define IKFAST_NO_MAIN // Don't include main() from IKFast @@ -68,49 +73,77 @@ const static std::string ABB_TIP_LINK ="tool0"; /// 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 +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 }; // Code generated by IKFast56/61 @@ -132,11 +165,10 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase const std::vector& getLinkNames() const { return link_names_; } public: - /** @class * @brief Interface for an IKFast kinematics plugin */ - IKFastKinematicsPlugin():active_(false){} + IKFastKinematicsPlugin() : active_(false) {} /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it @@ -148,83 +180,88 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase */ // Returns the first IK solution that is within joint limits, this is called by get_ik() service - 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; + 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, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * @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; + 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 + * @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; + 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 + * @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; + 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 + * @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; + 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 * - * This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, + * This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' + *node, * otherwise ROS TF is used to calculate the forward kinematics * * @param link_names A set of links for which FK needs to be computed @@ -232,55 +269,55 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @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; + bool getPositionFK(const std::vector& link_names, + const std::vector& joint_angles, + std::vector& poses) const; protected: - - bool initialize(const std::string &robot_description, - const std::string& group_name, - const std::string& base_name, - const std::string& tip_name, + 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; + 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; - - 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; + void getSolution(const IkSolutionList& solutions, 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; }; // 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) +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, ABB_BASE_LINK, ABB_TIP_LINK, search_discretization); - ros::NodeHandle node_handle("~/"+group_name); + ros::NodeHandle node_handle("~/" + group_name); std::string robot; - node_handle.param("robot",robot,std::string()); + node_handle.param("robot", robot, std::string()); // IKFast56/61 - fillFreeParams( GetNumFreeParameters(), GetFreeParameters() ); + fillFreeParams(GetNumFreeParameters(), GetFreeParameters()); num_joints_ = GetNumJoints(); - if(free_params_.size() > 1) + if (free_params_.size() > 1) { ROS_FATAL("Only one free joint paramter supported!"); return false; @@ -289,44 +326,47 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, urdf::Model robot_model; std::string xml_string; - std::string urdf_xml,full_urdf_xml; - node_handle.param("urdf_xml",urdf_xml,robot_description); - node_handle.searchParam(urdf_xml,full_urdf_xml); + std::string urdf_xml, full_urdf_xml; + node_handle.param("urdf_xml", urdf_xml, robot_description); + node_handle.searchParam(urdf_xml, full_urdf_xml); - ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server"); + ROS_DEBUG_NAMED("ikfast", "Reading xml file from parameter server"); if (!node_handle.getParam(full_urdf_xml, xml_string)) { - ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str()); + ROS_FATAL_NAMED("ikfast", "Could not load the xml from parameter server: %s", urdf_xml.c_str()); return false; } - node_handle.param(full_urdf_xml,xml_string,std::string()); + node_handle.param(full_urdf_xml, xml_string, std::string()); robot_model.initString(xml_string); - ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Reading joints and links from URDF"); - boost::shared_ptr link = boost::const_pointer_cast(robot_model.getLink(getTipFrame())); - while(link->name != base_frame_ && joint_names_.size() <= num_joints_) + boost::shared_ptr link = + boost::const_pointer_cast(robot_model.getLink(getTipFrame())); + while (link->name != base_frame_ && joint_names_.size() <= num_joints_) { - ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str()); + ROS_DEBUG_NAMED("ikfast", "Link %s", link->name.c_str()); link_names_.push_back(link->name); boost::shared_ptr joint = link->parent_joint; - if(joint) + if (joint) { if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { - ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name ); + ROS_DEBUG_STREAM_NAMED("ikfast", "Adding joint " << joint->name); joint_names_.push_back(joint->name); float lower, upper; int hasLimits; - if ( joint->type != urdf::Joint::CONTINUOUS ) + if (joint->type != urdf::Joint::CONTINUOUS) { - if(joint->safety) + if (joint->safety) { lower = joint->safety->soft_lower_limit; upper = joint->safety->soft_upper_limit; - } else { + } + else + { lower = joint->limits->lower; upper = joint->limits->upper; } @@ -338,7 +378,7 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, upper = M_PI; hasLimits = 0; } - if(hasLimits) + if (hasLimits) { joint_has_limits_vector_.push_back(true); joint_min_vector_.push_back(lower); @@ -351,39 +391,45 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, joint_max_vector_.push_back(M_PI); } } - } else + } + else { - ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str()); + ROS_WARN_NAMED("ikfast", "no joint corresponding to %s", link->name.c_str()); } link = link->getParent(); } - if(joint_names_.size() != num_joints_) + if (joint_names_.size() != num_joints_) { - ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_); + ROS_FATAL_STREAM_NAMED("ikfast", "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(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 &vfree, IkSolutionList &solutions) const +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[0] = pose_frame.p[0]; //-.18; trans[1] = pose_frame.p[1]; trans[2] = pose_frame.p[2]; @@ -392,99 +438,110 @@ int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector 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**, and **TranslationZAxisAngle4D**, the first value represents the angle. - ROS_ERROR_NAMED("ikfast", "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("ikfast", "IK for this IkParameterizationType not implemented yet."); - return 0; - - case IKP_Rotation3D: - case IKP_Lookat3D: - case IKP_TranslationXY2D: - case IKP_TranslationXYOrientation3D: - case IKP_TranslationXAxisAngleZNorm4D: - case IKP_TranslationYAxisAngleXNorm4D: - case IKP_TranslationZAxisAngleYNorm4D: - ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); - return 0; - - default: - ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?"); - return 0; + 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**, and + // **TranslationZAxisAngle4D**, the first value represents the angle. + ROS_ERROR_NAMED("ikfast", "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("ikfast", "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_Rotation3D: + case IKP_Lookat3D: + case IKP_TranslationXY2D: + case IKP_TranslationXYOrientation3D: + case IKP_TranslationXAxisAngleZNorm4D: + case IKP_TranslationYAxisAngleXNorm4D: + case IKP_TranslationZAxisAngleYNorm4D: + ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); + return 0; + + default: + ROS_ERROR_NAMED("ikfast", "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 +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::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); // std::cout << "solution " << i << ":" ; // for(int j=0;j &ik_seed_state, std::vector &solution) const +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) + 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(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; } - 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]); } @@ -514,48 +571,53 @@ double IKFastKinematicsPlugin::harmonize(const std::vector &ik_seed_stat // } // } -void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList &solutions, const std::vector &ik_seed_state, std::vector &solution) const +void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList& solutions, + const std::vector& ik_seed_state, + std::vector& solution) const { double mindist = DBL_MAX; int minindex = -1; std::vector sol; // IKFast56/61 - for(size_t i=0; i < solutions.GetNumSolutions(); ++i) + for (size_t i = 0; i < solutions.GetNumSolutions(); ++i) { - getSolution(solutions, i,sol); + getSolution(solutions, i, sol); double dist = harmonize(ik_seed_state, sol); - ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist); - //std::cout << "dist[" << i << "]= " << dist << std::endl; - if(minindex == -1 || dist &ik_seed_state, - double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const +bool IKFastKinematicsPlugin::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) const { - const IKCallbackFn solution_callback = 0; + 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); + 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 + +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); + 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 +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); + 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 +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("ikfast","searchPositionIK"); + ROS_DEBUG_STREAM_NAMED("ikfast", "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) + if (free_params_.size() == 0) { - ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints"); + ROS_DEBUG_STREAM_NAMED("ikfast", "No need to search since no free params/redundant joints"); // Find first IK solution, within joint limits - if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code)) + if (!getPositionIK(ik_pose, ik_seed_state, solution, error_code)) { - ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever"); + ROS_DEBUG_STREAM_NAMED("ikfast", "No solution whatsoever"); error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } // check for collisions if a callback is provided - if( !solution_callback.empty() ) + if (!solution_callback.empty()) { solution_callback(ik_pose, solution, error_code); - if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) + if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback"); return true; } else { - ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code); + ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code); return false; } } @@ -730,33 +768,36 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose // ------------------------------------------------------------------------------------------------- // Error Checking - if(!active_) + if (!active_) { - ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active"); + ROS_ERROR_STREAM_NAMED("ikfast", "Kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } - if(ik_seed_state.size() != num_joints_) + if (ik_seed_state.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED("ikfast", "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_) + if (!consistency_limits.empty() && consistency_limits.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size()); + ROS_ERROR_STREAM_NAMED("ikfast", "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); + tf::poseMsgToKDL(ik_pose, frame); std::vector vfree(free_params_.size()); @@ -771,66 +812,78 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose int num_positive_increments; int num_negative_increments; - if(!consistency_limits.empty()) + 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]]); + 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_); + 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_; + 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("ikfast","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("ikfast", "Large search space, consider increasing the search discretization"); - + ROS_DEBUG_STREAM_NAMED("ikfast", "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("ikfast", + "Large search space, consider increasing the search discretization"); + double best_costs = -1.0; std::vector best_solution; int nattempts = 0, nvalid = 0; - while(true) + while (true) { IkSolutionList solutions; - int numsol = solve(frame,vfree, solutions); + int numsol = solve(frame, vfree, solutions); - ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast"); - //ROS_INFO("%f",vfree[0]); + // ROS_INFO("%f",vfree[0]); - if( numsol > 0 ) + if (numsol > 0) { - for(int s = 0; s < numsol; ++s) + for (int s = 0; s < numsol; ++s) { nattempts++; std::vector sol; - getSolution(solutions,s,sol); + getSolution(solutions, s, sol); bool obeys_limits = true; - for(unsigned int i = 0; i < sol.size(); i++) + 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])) + 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("ikfast","Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); + // ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " << + // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << + // joint_max_vector_[i]); } - if(obeys_limits) + if (obeys_limits) { - getSolution(solutions,s,solution); + getSolution(solutions, s, solution); // This solution is within joint limits, now check if in collision (if callback provided) - if(!solution_callback.empty()) + if (!solution_callback.empty()) { solution_callback(ik_pose, solution, error_code); } @@ -839,14 +892,14 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose error_code.val = error_code.SUCCESS; } - if(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++) + for (unsigned int i = 0; i < solution.size(); i++) { double d = fabs(ik_seed_state[i] - solution[i]); if (d > costs) @@ -866,7 +919,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose } } - if(!getCount(counter, num_positive_increments, -num_negative_increments)) + if (!getCount(counter, num_positive_increments, -num_negative_increments)) { // Everything searched error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; @@ -880,8 +933,9 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose break; } - vfree[0] = initial_guess+search_discretization_*counter; - //ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]); + vfree[0] = initial_guess + search_discretization_ * counter; + // ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " + // << vfree[0]); } ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts); @@ -899,61 +953,66 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose } // 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 +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("ikfast","getPositionIK"); + ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK"); - if(!active_) + if (!active_) { - ROS_ERROR("kinematics not active"); + ROS_ERROR("kinematics not active"); return false; } std::vector vfree(free_params_.size()); - for(std::size_t i = 0; i < free_params_.size(); ++i) + 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 + 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); + tf::poseMsgToKDL(ik_pose, frame); IkSolutionList solutions; - int numsol = solve(frame,vfree,solutions); + int numsol = solve(frame, vfree, solutions); - ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast"); - if(numsol) + if (numsol) { - for(int s = 0; s < numsol; ++s) + for (int s = 0; s < numsol; ++s) { std::vector sol; - getSolution(solutions,s,sol); - ROS_DEBUG_NAMED("ikfast","Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]); + getSolution(solutions, s, sol); + ROS_DEBUG_NAMED("ikfast", "Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], + sol[3], sol[4], sol[5]); bool obeys_limits = true; - for(unsigned int i = 0; i < sol.size(); i++) + 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)) ) ) + 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("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + ROS_DEBUG_STREAM_NAMED("ikfast", "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) + if (obeys_limits) { // All elements of solution obey limits - getSolution(solutions,s,solution); + getSolution(solutions, s, solution); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; return true; } @@ -961,13 +1020,11 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, } else { - ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution"); + ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution"); } error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } - - } // end namespace diff --git a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_solver.hpp b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_solver.hpp index 1d228dce..30321e61 100644 --- a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_solver.hpp +++ b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/abb_irb2400_manipulator_ikfast_solver.hpp @@ -5,7 +5,7 @@ /// 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. @@ -16,14 +16,15 @@ /// 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 +/// 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==61); +IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61); #include #include @@ -49,7 +50,16 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #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()); } } +#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 @@ -59,9 +69,9 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #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) +#define IK2PI ((IkReal)6.28318530717959) +#define IKPI ((IkReal)3.14159265358979) +#define IKPI_2 ((IkReal)1.57079632679490) #ifdef _MSC_VER #ifndef isnan @@ -71,25 +81,32 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); // 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); +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 { +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 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); } @@ -111,50 +128,68 @@ inline double IKlog(double f) { return log(f); } 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); + 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); + 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); + 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); + 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); + 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); + 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); } @@ -162,120 +197,171 @@ 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 IKatan2(float fy, float fx) { - if( std::isnan(fy) ) { - IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned - return float(IKPI_2); - } - else if( std::isnan(fx) ) { - return 0; - } - return atan2f(fy,fx); +inline float IKsqrt(float f) +{ + if (f <= 0.0f) + return 0.0f; + return sqrtf(f); } -inline double IKatan2(double fy, double fx) { - if( std::isnan(fy) ) { - IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned - return IKPI_2; - } - else if( std::isnan(fx) ) { - return 0; - } - return atan2(fy,fx); +inline double IKsqrt(double f) +{ + if (f <= 0.0) + return 0.0; + return sqrt(f); } - -inline float IKsign(float f) { - if( f > 0 ) { - return float(1); - } - else if( f < 0 ) { - return float(-1); - } +inline float IKatan2(float fy, float fx) +{ + if (std::isnan(fy)) + { + IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned + return float(IKPI_2); + } + else if (std::isnan(fx)) + { + return 0; + } + return atan2f(fy, fx); +} +inline double IKatan2(double fy, double fx) +{ + if (std::isnan(fy)) + { + IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned + return IKPI_2; + } + else if (std::isnan(fx)) + { return 0; + } + return atan2(fy, fx); } -inline double IKsign(double f) { - if( f > 0 ) { - return 1.0; - } - else if( f < 0 ) { - return -1.0; - } - return 0; +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; } /// 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,x43,x44,x45,x46,x47,x48,x49; -x0=IKcos(j[0]); -x1=IKcos(j[3]); -x2=IKsin(j[0]); -x3=IKsin(j[3]); -x4=IKsin(j[1]); -x5=IKsin(j[2]); -x6=IKcos(j[1]); -x7=IKcos(j[2]); -x8=IKsin(j[5]); -x9=IKsin(j[4]); -x10=IKcos(j[4]); -x11=IKcos(j[5]); -x12=((IkReal(0.0850000000000000))*(x1)); -x13=((IkReal(0.135000000000000))*(x7)); -x14=((IkReal(0.0850000000000000))*(x3)); -x15=((IkReal(1.00000000000000))*(x7)); -x16=((IkReal(1.00000000000000))*(x2)); -x17=((IkReal(0.0850000000000000))*(x5)); -x18=((IkReal(1.00000000000000))*(x9)); -x19=((IkReal(1.00000000000000))*(x10)); -x20=((IkReal(1.00000000000000))*(x0)); -x21=((IkReal(0.755000000000000))*(x5)); -x22=((IkReal(1.00000000000000))*(x5)); -x23=((x0)*(x4)); -x24=((x6)*(x7)); -x25=((x5)*(x6)); -x26=((x2)*(x4)); -x27=((x4)*(x5)); -x28=((x1)*(x9)); -x29=((x4)*(x7)); -x30=((x15)*(x6)); -x31=((x20)*(x27)); -x32=((x16)*(x27)); -x33=((x27)+(((IkReal(-1.00000000000000))*(x30)))); -x34=((x30)+(((IkReal(-1.00000000000000))*(x22)*(x4)))); -x35=((((x22)*(x6)))+(((x15)*(x4)))); -x36=((x1)*(x33)); -x37=((x3)*(x34)); -x38=((x10)*(x36)); -x39=((((IkReal(-1.00000000000000))*(x0)*(x30)))+(x31)); -x40=((((IkReal(-1.00000000000000))*(x2)*(x30)))+(x32)); -x41=((((x15)*(x23)))+(((x20)*(x25)))); -x42=((IkReal(-1.00000000000000))*(x41)); -x43=((((x15)*(x26)))+(((x16)*(x25)))); -x44=((IkReal(-1.00000000000000))*(x43)); -x45=((((x1)*(x42)))+(((IkReal(-1.00000000000000))*(x16)*(x3)))); -x46=((((x3)*(x41)))+(((IkReal(-1.00000000000000))*(x1)*(x16)))); -x47=((((x3)*(x43)))+(((x0)*(x1)))); -x48=((((x0)*(x3)))+(((x1)*(x44)))); -x49=((x10)*(x45)); -eerot[0]=((((x46)*(x8)))+(((x11)*(((x49)+(((x39)*(x9)))))))); -eerot[1]=((((x8)*(((((IkReal(-1.00000000000000))*(x18)*(x39)))+(((IkReal(-1.00000000000000))*(x19)*(x45)))))))+(((x11)*(x46)))); -eerot[2]=((((x45)*(x9)))+(((x10)*(((((IkReal(-1.00000000000000))*(x31)))+(((x0)*(x24)))))))); -IkReal x50=((x0)*(x24)); -IkReal x51=((IkReal(1.00000000000000))*(x23)); -eetrans[0]=((((IkReal(0.135000000000000))*(x0)*(x25)))+(((IkReal(0.755000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x21)*(x51)))+(((x10)*(((((IkReal(0.0850000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x17)*(x51)))))))+(((x13)*(x23)))+(((IkReal(0.100000000000000))*(x0)))+(((x9)*(((((x12)*(x42)))+(((IkReal(-1.00000000000000))*(x14)*(x2)))))))+(((IkReal(0.705000000000000))*(x23)))); -eerot[3]=((((x47)*(x8)))+(((x11)*(((((x40)*(x9)))+(((x10)*(x48)))))))); -eerot[4]=((((x11)*(x47)))+(((x8)*(((((IkReal(-1.00000000000000))*(x18)*(x40)))+(((IkReal(-1.00000000000000))*(x19)*(x48)))))))); -eerot[5]=((((x48)*(x9)))+(((x10)*(((((IkReal(-1.00000000000000))*(x32)))+(((x2)*(x24)))))))); -IkReal x52=((x2)*(x24)); -IkReal x53=((IkReal(1.00000000000000))*(x26)); -eetrans[1]=((((IkReal(0.755000000000000))*(x52)))+(((x13)*(x26)))+(((x10)*(((((IkReal(0.0850000000000000))*(x52)))+(((IkReal(-1.00000000000000))*(x17)*(x53)))))))+(((x9)*(((((x0)*(x14)))+(((x12)*(x44)))))))+(((IkReal(0.100000000000000))*(x2)))+(((IkReal(-1.00000000000000))*(x21)*(x53)))+(((IkReal(0.705000000000000))*(x26)))+(((IkReal(0.135000000000000))*(x2)*(x25)))); -eerot[6]=((((x11)*(((((x35)*(x9)))+(x38)))))+(((x37)*(x8)))); -eerot[7]=((((x11)*(x37)))+(((x8)*(((((IkReal(-1.00000000000000))*(x18)*(x35)))+(((IkReal(-1.00000000000000))*(x19)*(x36)))))))); -eerot[8]=((((IkReal(-1.00000000000000))*(x10)*(x35)))+(((x28)*(x33)))); -IkReal x54=((IkReal(1.00000000000000))*(x6)); -eetrans[2]=((IkReal(0.615000000000000))+(((x10)*(((((IkReal(-0.0850000000000000))*(x29)))+(((IkReal(-1.00000000000000))*(x17)*(x54)))))))+(((x13)*(x6)))+(((x28)*(((((IkReal(-0.0850000000000000))*(x24)))+(((x17)*(x4)))))))+(((IkReal(-1.00000000000000))*(x21)*(x54)))+(((IkReal(0.705000000000000))*(x6)))+(((IkReal(-0.755000000000000))*(x29)))+(((IkReal(-0.135000000000000))*(x27)))); +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, x43, x44, x45, x46, x47, x48, x49; + x0 = IKcos(j[0]); + x1 = IKcos(j[3]); + x2 = IKsin(j[0]); + x3 = IKsin(j[3]); + x4 = IKsin(j[1]); + x5 = IKsin(j[2]); + x6 = IKcos(j[1]); + x7 = IKcos(j[2]); + x8 = IKsin(j[5]); + x9 = IKsin(j[4]); + x10 = IKcos(j[4]); + x11 = IKcos(j[5]); + x12 = ((IkReal(0.0850000000000000)) * (x1)); + x13 = ((IkReal(0.135000000000000)) * (x7)); + x14 = ((IkReal(0.0850000000000000)) * (x3)); + x15 = ((IkReal(1.00000000000000)) * (x7)); + x16 = ((IkReal(1.00000000000000)) * (x2)); + x17 = ((IkReal(0.0850000000000000)) * (x5)); + x18 = ((IkReal(1.00000000000000)) * (x9)); + x19 = ((IkReal(1.00000000000000)) * (x10)); + x20 = ((IkReal(1.00000000000000)) * (x0)); + x21 = ((IkReal(0.755000000000000)) * (x5)); + x22 = ((IkReal(1.00000000000000)) * (x5)); + x23 = ((x0) * (x4)); + x24 = ((x6) * (x7)); + x25 = ((x5) * (x6)); + x26 = ((x2) * (x4)); + x27 = ((x4) * (x5)); + x28 = ((x1) * (x9)); + x29 = ((x4) * (x7)); + x30 = ((x15) * (x6)); + x31 = ((x20) * (x27)); + x32 = ((x16) * (x27)); + x33 = ((x27) + (((IkReal(-1.00000000000000)) * (x30)))); + x34 = ((x30) + (((IkReal(-1.00000000000000)) * (x22) * (x4)))); + x35 = ((((x22) * (x6))) + (((x15) * (x4)))); + x36 = ((x1) * (x33)); + x37 = ((x3) * (x34)); + x38 = ((x10) * (x36)); + x39 = ((((IkReal(-1.00000000000000)) * (x0) * (x30))) + (x31)); + x40 = ((((IkReal(-1.00000000000000)) * (x2) * (x30))) + (x32)); + x41 = ((((x15) * (x23))) + (((x20) * (x25)))); + x42 = ((IkReal(-1.00000000000000)) * (x41)); + x43 = ((((x15) * (x26))) + (((x16) * (x25)))); + x44 = ((IkReal(-1.00000000000000)) * (x43)); + x45 = ((((x1) * (x42))) + (((IkReal(-1.00000000000000)) * (x16) * (x3)))); + x46 = ((((x3) * (x41))) + (((IkReal(-1.00000000000000)) * (x1) * (x16)))); + x47 = ((((x3) * (x43))) + (((x0) * (x1)))); + x48 = ((((x0) * (x3))) + (((x1) * (x44)))); + x49 = ((x10) * (x45)); + eerot[0] = ((((x46) * (x8))) + (((x11) * (((x49) + (((x39) * (x9)))))))); + eerot[1] = ((((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x39))) + + (((IkReal(-1.00000000000000)) * (x19) * (x45))))))) + + (((x11) * (x46)))); + eerot[2] = ((((x45) * (x9))) + + (((x10) * (((((IkReal(-1.00000000000000)) * (x31))) + (((x0) * (x24)))))))); + IkReal x50 = ((x0) * (x24)); + IkReal x51 = ((IkReal(1.00000000000000)) * (x23)); + eetrans[0] = + ((((IkReal(0.135000000000000)) * (x0) * (x25))) + (((IkReal(0.755000000000000)) * (x50))) + + (((IkReal(-1.00000000000000)) * (x21) * (x51))) + + (((x10) * (((((IkReal(0.0850000000000000)) * (x50))) + + (((IkReal(-1.00000000000000)) * (x17) * (x51))))))) + + (((x13) * (x23))) + (((IkReal(0.100000000000000)) * (x0))) + + (((x9) * (((((x12) * (x42))) + (((IkReal(-1.00000000000000)) * (x14) * (x2))))))) + + (((IkReal(0.705000000000000)) * (x23)))); + eerot[3] = ((((x47) * (x8))) + (((x11) * (((((x40) * (x9))) + (((x10) * (x48)))))))); + eerot[4] = ((((x11) * (x47))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x40))) + + (((IkReal(-1.00000000000000)) * (x19) * (x48)))))))); + eerot[5] = ((((x48) * (x9))) + + (((x10) * (((((IkReal(-1.00000000000000)) * (x32))) + (((x2) * (x24)))))))); + IkReal x52 = ((x2) * (x24)); + IkReal x53 = ((IkReal(1.00000000000000)) * (x26)); + eetrans[1] = + ((((IkReal(0.755000000000000)) * (x52))) + (((x13) * (x26))) + + (((x10) * (((((IkReal(0.0850000000000000)) * (x52))) + + (((IkReal(-1.00000000000000)) * (x17) * (x53))))))) + + (((x9) * (((((x0) * (x14))) + (((x12) * (x44))))))) + + (((IkReal(0.100000000000000)) * (x2))) + (((IkReal(-1.00000000000000)) * (x21) * (x53))) + + (((IkReal(0.705000000000000)) * (x26))) + (((IkReal(0.135000000000000)) * (x2) * (x25)))); + eerot[6] = ((((x11) * (((((x35) * (x9))) + (x38))))) + (((x37) * (x8)))); + eerot[7] = ((((x11) * (x37))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x18) * (x35))) + + (((IkReal(-1.00000000000000)) * (x19) * (x36)))))))); + eerot[8] = ((((IkReal(-1.00000000000000)) * (x10) * (x35))) + (((x28) * (x33)))); + IkReal x54 = ((IkReal(1.00000000000000)) * (x6)); + eetrans[2] = + ((IkReal(0.615000000000000)) + + (((x10) * (((((IkReal(-0.0850000000000000)) * (x29))) + + (((IkReal(-1.00000000000000)) * (x17) * (x54))))))) + + (((x13) * (x6))) + + (((x28) * (((((IkReal(-0.0850000000000000)) * (x24))) + (((x17) * (x4))))))) + + (((IkReal(-1.00000000000000)) * (x21) * (x54))) + (((IkReal(0.705000000000000)) * (x6))) + + (((IkReal(-0.755000000000000)) * (x29))) + (((IkReal(-0.135000000000000)) * (x27)))); } IKFAST_API int GetNumFreeParameters() { return 0; } @@ -286,2264 +372,3237 @@ IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } IKFAST_API int GetIkType() { return 0x67000001; } -class IKSolver { -public: -IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,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; - -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=((((IkReal(-0.0850000000000000))*(r02)))+(px)); -new_r10=r10; -new_r11=r11; -new_r12=r12; -new_py=((((IkReal(-0.0850000000000000))*(r12)))+(py)); -new_r20=r20; -new_r21=r21; -new_r22=r22; -new_pz=((IkReal(-0.615000000000000))+(((IkReal(-0.0850000000000000))*(r22)))+(pz)); -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; -pp=(((px)*(px))+((pz)*(pz))+((py)*(py))); -npx=((((py)*(r10)))+(((pz)*(r20)))+(((px)*(r00)))); -npy=((((px)*(r01)))+(((pz)*(r21)))+(((py)*(r11)))); -npz=((((py)*(r12)))+(((pz)*(r22)))+(((px)*(r02)))); -rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10)))); -rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00)))); -rxp0_2=((((py)*(r00)))+(((IkReal(-1.00000000000000))*(px)*(r10)))); -rxp1_0=((((pz)*(r11)))+(((IkReal(-1.00000000000000))*(py)*(r21)))); -rxp1_1=((((IkReal(-1.00000000000000))*(pz)*(r01)))+(((px)*(r21)))); -rxp1_2=((((py)*(r01)))+(((IkReal(-1.00000000000000))*(px)*(r11)))); -rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12)))); -rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02)))); -rxp2_2=((((py)*(r02)))+(((IkReal(-1.00000000000000))*(px)*(r12)))); -{ -IkReal j0array[2], cj0array[2], sj0array[2]; -bool j0valid[2]={false}; -_nj0 = 2; -if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x55=IKatan2(((IkReal(-1.00000000000000))*(py)), px); -j0array[0]=((IkReal(-1.00000000000000))*(x55)); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x55)))); -sj0array[1]=IKsin(j0array[1]); -cj0array[1]=IKcos(j0array[1]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -if( j0array[1] > IKPI ) -{ - j0array[1]-=IK2PI; -} -else if( j0array[1] < -IKPI ) -{ j0array[1]+=IK2PI; -} -j0valid[1] = true; -for(int ij0 = 0; ij0 < 2; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 2; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; - -{ -IkReal j2array[2], cj2array[2], sj2array[2]; -bool j2valid[2]={false}; -_nj2 = 2; -if( (((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(-0.924698002368864))*(pp))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(-0.924698002368864))*(pp))))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x56=IKasin(((IkReal(0.994304644497180))+(((IkReal(0.184939600473773))*(py)*(sj0)))+(((IkReal(0.184939600473773))*(cj0)*(px)))+(((IkReal(-0.924698002368864))*(pp))))); -j2array[0]=((IkReal(-2.96465459743209))+(((IkReal(-1.00000000000000))*(x56)))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -j2array[1]=((IkReal(0.176938056157703))+(x56)); -sj2array[1]=IKsin(j2array[1]); -cj2array[1]=IKcos(j2array[1]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -if( j2array[1] > IKPI ) -{ - j2array[1]-=IK2PI; -} -else if( j2array[1] < -IKPI ) -{ j2array[1]+=IK2PI; -} -j2valid[1] = true; -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 dummyeval[1]; -IkReal gconst1; -IkReal x57=((IkReal(0.755000000000000))*(cj2)); -IkReal x58=((py)*(sj0)); -IkReal x59=((cj0)*(px)); -IkReal x60=((IkReal(0.135000000000000))*(sj2)); -gconst1=IKsign(((((IkReal(0.705000000000000))*(pz)))+(((IkReal(-1.00000000000000))*(x58)*(x60)))+(((IkReal(0.0755000000000000))*(cj2)))+(((IkReal(-0.755000000000000))*(pz)*(sj2)))+(((IkReal(-1.00000000000000))*(x57)*(x58)))+(((IkReal(-1.00000000000000))*(x59)*(x60)))+(((IkReal(-1.00000000000000))*(x57)*(x59)))+(((IkReal(0.0135000000000000))*(sj2)))+(((IkReal(0.135000000000000))*(cj2)*(pz))))); -IkReal x61=((IkReal(10.0000000000000))*(sj2)); -IkReal x62=((cj0)*(px)); -IkReal x63=((py)*(sj0)); -IkReal x64=((IkReal(55.9259259259259))*(cj2)); -dummyeval[0]=((((IkReal(52.2222222222222))*(pz)))+(((IkReal(-1.00000000000000))*(x62)*(x64)))+(sj2)+(((IkReal(10.0000000000000))*(cj2)*(pz)))+(((IkReal(-1.00000000000000))*(x63)*(x64)))+(((IkReal(-1.00000000000000))*(x61)*(x62)))+(((IkReal(-1.00000000000000))*(x61)*(x63)))+(((IkReal(5.59259259259259))*(cj2)))+(((IkReal(-55.9259259259259))*(pz)*(sj2)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst0; -IkReal x65=((IkReal(0.755000000000000))*(sj2)); -IkReal x66=((cj0)*(px)); -IkReal x67=((py)*(sj0)); -IkReal x68=((IkReal(0.135000000000000))*(cj2)); -gconst0=IKsign(((IkReal(0.0705000000000000))+(((x65)*(x66)))+(((IkReal(-0.705000000000000))*(x67)))+(((x65)*(x67)))+(((IkReal(-0.755000000000000))*(cj2)*(pz)))+(((IkReal(-0.705000000000000))*(x66)))+(((IkReal(-1.00000000000000))*(x66)*(x68)))+(((IkReal(-1.00000000000000))*(x67)*(x68)))+(((IkReal(0.0135000000000000))*(cj2)))+(((IkReal(-0.135000000000000))*(pz)*(sj2)))+(((IkReal(-0.0755000000000000))*(sj2))))); -IkReal x69=((cj0)*(px)); -IkReal x70=((IkReal(10.0000000000000))*(cj2)); -IkReal x71=((IkReal(55.9259259259259))*(sj2)); -IkReal x72=((py)*(sj0)); -dummyeval[0]=((IkReal(5.22222222222222))+(((IkReal(-1.00000000000000))*(x70)*(x72)))+(((x71)*(x72)))+(((IkReal(-52.2222222222222))*(x72)))+(((IkReal(-10.0000000000000))*(pz)*(sj2)))+(cj2)+(((IkReal(-52.2222222222222))*(x69)))+(((IkReal(-1.00000000000000))*(x69)*(x70)))+(((IkReal(-5.59259259259259))*(sj2)))+(((IkReal(-55.9259259259259))*(cj2)*(pz)))+(((x69)*(x71)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j1array[1], cj1array[1], sj1array[1]; -bool j1valid[1]={false}; -_nj1 = 1; -IkReal x73=(sj2)*(sj2); -IkReal x74=(cj2)*(cj2); -IkReal x75=((cj2)*(sj2)); -IkReal x76=((IkReal(1.00000000000000))*(pz)); -if( IKabs(((gconst0)*(((IkReal(-0.497025000000000))+(((IkReal(-0.190350000000000))*(cj2)))+((pz)*(pz))+(((IkReal(1.06455000000000))*(sj2)))+(((IkReal(0.203850000000000))*(x75)))+(((IkReal(-0.570025000000000))*(x73)))+(((IkReal(-0.0182250000000000))*(x74))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(0.101925000000000))*(x73)))+(((IkReal(-0.101925000000000))*(x74)))+(((IkReal(0.100000000000000))*(pz)))+(((IkReal(0.551800000000000))*(x75)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x76)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x76))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j1array[0]=IKatan2(((gconst0)*(((IkReal(-0.497025000000000))+(((IkReal(-0.190350000000000))*(cj2)))+((pz)*(pz))+(((IkReal(1.06455000000000))*(sj2)))+(((IkReal(0.203850000000000))*(x75)))+(((IkReal(-0.570025000000000))*(x73)))+(((IkReal(-0.0182250000000000))*(x74)))))), ((gconst0)*(((((IkReal(0.101925000000000))*(x73)))+(((IkReal(-0.101925000000000))*(x74)))+(((IkReal(0.100000000000000))*(pz)))+(((IkReal(0.551800000000000))*(x75)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x76)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x76))))))); -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 x77=IKsin(j1); -IkReal x78=IKcos(j1); -IkReal x79=((IkReal(0.135000000000000))*(sj2)); -IkReal x80=((cj0)*(px)); -IkReal x81=((IkReal(0.755000000000000))*(sj2)); -IkReal x82=((py)*(sj0)); -IkReal x83=((IkReal(0.755000000000000))*(cj2)); -IkReal x84=((IkReal(0.135000000000000))*(cj2)); -IkReal x85=((IkReal(0.135000000000000))*(x78)); -IkReal x86=((IkReal(1.00000000000000))*(x78)); -IkReal x87=((IkReal(1.41000000000000))*(x77)); -IkReal x88=((pz)*(x78)); -evalcond[0]=((IkReal(-0.705000000000000))+(((x77)*(x80)))+(((x77)*(x82)))+(((IkReal(-0.100000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(x84)))+(x88)+(x81)); -evalcond[1]=((((IkReal(0.100000000000000))*(x78)))+(((IkReal(-1.00000000000000))*(x80)*(x86)))+(x79)+(((IkReal(-1.00000000000000))*(x82)*(x86)))+(x83)+(((pz)*(x77)))); -evalcond[2]=((((IkReal(-0.705000000000000))*(x78)))+(((IkReal(-1.00000000000000))*(x78)*(x84)))+(((x77)*(x79)))+(pz)+(((x78)*(x81)))+(((x77)*(x83)))); -evalcond[3]=((IkReal(0.0812250000000000))+(((IkReal(-0.141000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(pp)))+(((x82)*(x87)))+(((IkReal(0.200000000000000))*(x82)))+(((IkReal(1.41000000000000))*(x88)))+(((x80)*(x87)))+(((IkReal(0.200000000000000))*(x80)))); -evalcond[4]=((IkReal(0.100000000000000))+(((IkReal(-1.00000000000000))*(x82)))+(((x77)*(x84)))+(((IkReal(-1.00000000000000))*(x77)*(x81)))+(((x78)*(x83)))+(((x78)*(x79)))+(((IkReal(0.705000000000000))*(x77)))+(((IkReal(-1.00000000000000))*(x80)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j1array[1], cj1array[1], sj1array[1]; -bool j1valid[1]={false}; -_nj1 = 1; -IkReal x228=(sj2)*(sj2); -IkReal x229=(cj2)*(cj2); -IkReal x230=((cj2)*(sj2)); -if( IKabs(((gconst1)*(((((IkReal(0.551800000000000))*(x230)))+(((IkReal(0.101925000000000))*(x228)))+(((IkReal(-0.100000000000000))*(pz)))+(((IkReal(-0.101925000000000))*(x229)))+(((cj0)*(px)*(pz)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((py)*(pz)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-0.0182250000000000))*(x228)))+((pz)*(pz))+(((IkReal(-0.203850000000000))*(x230)))+(((IkReal(-0.570025000000000))*(x229))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j1array[0]=IKatan2(((gconst1)*(((((IkReal(0.551800000000000))*(x230)))+(((IkReal(0.101925000000000))*(x228)))+(((IkReal(-0.100000000000000))*(pz)))+(((IkReal(-0.101925000000000))*(x229)))+(((cj0)*(px)*(pz)))+(((IkReal(-0.532275000000000))*(cj2)))+(((IkReal(-0.0951750000000000))*(sj2)))+(((py)*(pz)*(sj0)))))), ((gconst1)*(((((IkReal(-0.0182250000000000))*(x228)))+((pz)*(pz))+(((IkReal(-0.203850000000000))*(x230)))+(((IkReal(-0.570025000000000))*(x229))))))); -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 x231=IKsin(j1); -IkReal x232=IKcos(j1); -IkReal x233=((IkReal(0.135000000000000))*(sj2)); -IkReal x234=((cj0)*(px)); -IkReal x235=((IkReal(0.755000000000000))*(sj2)); -IkReal x236=((py)*(sj0)); -IkReal x237=((IkReal(0.755000000000000))*(cj2)); -IkReal x238=((IkReal(0.135000000000000))*(cj2)); -IkReal x239=((IkReal(0.135000000000000))*(x232)); -IkReal x240=((IkReal(1.00000000000000))*(x232)); -IkReal x241=((IkReal(1.41000000000000))*(x231)); -IkReal x242=((pz)*(x232)); -evalcond[0]=((IkReal(-0.705000000000000))+(((IkReal(-1.00000000000000))*(x238)))+(x235)+(((x231)*(x236)))+(x242)+(((IkReal(-0.100000000000000))*(x231)))+(((x231)*(x234)))); -evalcond[1]=((((IkReal(0.100000000000000))*(x232)))+(((pz)*(x231)))+(x233)+(x237)+(((IkReal(-1.00000000000000))*(x234)*(x240)))+(((IkReal(-1.00000000000000))*(x236)*(x240)))); -evalcond[2]=((((IkReal(-0.705000000000000))*(x232)))+(((x231)*(x237)))+(((x231)*(x233)))+(pz)+(((x232)*(x235)))+(((IkReal(-1.00000000000000))*(x232)*(x238)))); -evalcond[3]=((IkReal(0.0812250000000000))+(((x236)*(x241)))+(((IkReal(0.200000000000000))*(x236)))+(((IkReal(0.200000000000000))*(x234)))+(((IkReal(-1.00000000000000))*(pp)))+(((x234)*(x241)))+(((IkReal(1.41000000000000))*(x242)))+(((IkReal(-0.141000000000000))*(x231)))); -evalcond[4]=((IkReal(0.100000000000000))+(((IkReal(-1.00000000000000))*(x231)*(x235)))+(((IkReal(0.705000000000000))*(x231)))+(((IkReal(-1.00000000000000))*(x236)))+(((x231)*(x238)))+(((x232)*(x233)))+(((x232)*(x237)))+(((IkReal(-1.00000000000000))*(x234)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} -} -} -} -return solutions.GetNumSolutions()>0; -} -inline void rotationfunction0(IkSolutionListBase& solutions) { -for(int rotationiter = 0; rotationiter < 1; ++rotationiter) { -IkReal x89=((cj0)*(r00)); -IkReal x90=((cj0)*(r01)); -IkReal x91=((sj1)*(sj2)); -IkReal x92=((IkReal(1.00000000000000))*(sj0)); -IkReal x93=((r10)*(sj0)); -IkReal x94=((IkReal(1.00000000000000))*(cj2)); -IkReal x95=((r12)*(sj0)); -IkReal x96=((cj0)*(r02)); -IkReal x97=((r11)*(sj0)); -IkReal x98=((((IkReal(-1.00000000000000))*(cj1)*(x94)))+(x91)); -IkReal x99=((((IkReal(-1.00000000000000))*(x91)))+(((cj1)*(cj2)))); -IkReal x100=((cj0)*(x99)); -IkReal x101=((((IkReal(-1.00000000000000))*(sj1)*(x94)))+(((IkReal(-1.00000000000000))*(cj1)*(sj2)))); -IkReal x102=((sj0)*(x101)); -new_r00=((((r20)*(x98)))+(((x101)*(x89)))+(((x101)*(x93)))); -new_r01=((((x101)*(x90)))+(((x101)*(x97)))+(((r21)*(x98)))); -new_r02=((((x101)*(x95)))+(((r22)*(x98)))+(((x101)*(x96)))); -new_r10=((((IkReal(-1.00000000000000))*(r00)*(x92)))+(((cj0)*(r10)))); -new_r11=((((IkReal(-1.00000000000000))*(r01)*(x92)))+(((cj0)*(r11)))); -new_r12=((((IkReal(-1.00000000000000))*(r02)*(x92)))+(((cj0)*(r12)))); -new_r20=((((x93)*(x99)))+(((x89)*(x99)))+(((r20)*(x101)))); -new_r21=((((r21)*(x101)))+(((x97)*(x99)))+(((x90)*(x99)))); -new_r22=((((x96)*(x99)))+(((r22)*(x101)))+(((x95)*(x99)))); -{ -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( std::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 dummyeval[1]; -IkReal gconst4; -gconst4=IKsign(sj4); -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst2; -gconst2=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst3; -gconst3=IKsign(((((new_r10)*(new_r12)*(sj4)))+(((new_r00)*(new_r02)*(sj4))))); -dummyeval[0]=((((new_r10)*(new_r12)*(sj4)))+(((new_r00)*(new_r02)*(sj4)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[7]; -IkReal x103=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=x103; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=new_r20; -evalcond[5]=new_r21; -evalcond[6]=x103; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 ) -{ -{ -IkReal j3array[2], cj3array[2], sj3array[2]; -bool j3valid[2]={false}; -_nj3 = 2; -if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x104=IKatan2(new_r02, new_r12); -j3array[0]=((IkReal(-1.00000000000000))*(x104)); -sj3array[0]=IKsin(j3array[0]); -cj3array[0]=IKcos(j3array[0]); -j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x104)))); -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]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3))))); -if( IKabs(evalcond[0]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(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[4]; -IkReal x105=IKsin(j5); -IkReal x106=((IkReal(1.00000000000000))*(sj3)); -IkReal x107=((IkReal(1.00000000000000))*(IKcos(j5))); -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x106)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x105)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x107)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x106)))); -evalcond[2]=((((new_r11)*(sj3)))+(x105)+(((cj3)*(new_r01)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x107)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -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 -{ -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[5]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[6]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 ) -{ -{ -IkReal j3array[2], cj3array[2], sj3array[2]; -bool j3valid[2]={false}; -_nj3 = 2; -if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x108=IKatan2(new_r02, new_r12); -j3array[0]=((IkReal(-1.00000000000000))*(x108)); -sj3array[0]=IKsin(j3array[0]); -cj3array[0]=IKcos(j3array[0]); -j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x108)))); -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]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3))))); -if( IKabs(evalcond[0]) > 0.000001 ) +class IKSolver { -continue; -} -} +public: + IkReal j0, cj0, sj0, htj0, j1, cj1, sj1, htj1, j2, cj2, sj2, htj2, j3, cj3, sj3, htj3, j4, cj4, + sj4, htj4, j5, cj5, sj5, htj5, 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; + + 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 = ((((IkReal(-0.0850000000000000)) * (r02))) + (px)); + new_r10 = r10; + new_r11 = r11; + new_r12 = r12; + new_py = ((((IkReal(-0.0850000000000000)) * (r12))) + (py)); + new_r20 = r20; + new_r21 = r21; + new_r22 = r22; + new_pz = ((IkReal(-0.615000000000000)) + (((IkReal(-0.0850000000000000)) * (r22))) + (pz)); + 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; + pp = (((px) * (px)) + ((pz) * (pz)) + ((py) * (py))); + npx = ((((py) * (r10))) + (((pz) * (r20))) + (((px) * (r00)))); + npy = ((((px) * (r01))) + (((pz) * (r21))) + (((py) * (r11)))); + npz = ((((py) * (r12))) + (((pz) * (r22))) + (((px) * (r02)))); + rxp0_0 = ((((IkReal(-1.00000000000000)) * (py) * (r20))) + (((pz) * (r10)))); + rxp0_1 = ((((px) * (r20))) + (((IkReal(-1.00000000000000)) * (pz) * (r00)))); + rxp0_2 = ((((py) * (r00))) + (((IkReal(-1.00000000000000)) * (px) * (r10)))); + rxp1_0 = ((((pz) * (r11))) + (((IkReal(-1.00000000000000)) * (py) * (r21)))); + rxp1_1 = ((((IkReal(-1.00000000000000)) * (pz) * (r01))) + (((px) * (r21)))); + rxp1_2 = ((((py) * (r01))) + (((IkReal(-1.00000000000000)) * (px) * (r11)))); + rxp2_0 = ((((IkReal(-1.00000000000000)) * (py) * (r22))) + (((pz) * (r12)))); + rxp2_1 = ((((px) * (r22))) + (((IkReal(-1.00000000000000)) * (pz) * (r02)))); + rxp2_2 = ((((py) * (r02))) + (((IkReal(-1.00000000000000)) * (px) * (r12)))); + { + IkReal j0array[2], cj0array[2], sj0array[2]; + bool j0valid[2] = {false}; + _nj0 = 2; + if (IKabs(((IkReal(-1.00000000000000)) * (py))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(px) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x55 = IKatan2(((IkReal(-1.00000000000000)) * (py)), px); + j0array[0] = ((IkReal(-1.00000000000000)) * (x55)); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + j0array[1] = ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x55)))); + sj0array[1] = IKsin(j0array[1]); + cj0array[1] = IKcos(j0array[1]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + if (j0array[1] > IKPI) + { + j0array[1] -= IK2PI; + } + else if (j0array[1] < -IKPI) + { + j0array[1] += IK2PI; + } + j0valid[1] = true; + for (int ij0 = 0; ij0 < 2; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 2; ++iij0) + { + if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + + { + IkReal j2array[2], cj2array[2], sj2array[2]; + bool j2valid[2] = {false}; + _nj2 = 2; + if ((((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) + + (((IkReal(0.184939600473773)) * (cj0) * (px))) + + (((IkReal(-0.924698002368864)) * (pp))))) < -1 - IKFAST_SINCOS_THRESH || + (((IkReal(0.994304644497180)) + (((IkReal(0.184939600473773)) * (py) * (sj0))) + + (((IkReal(0.184939600473773)) * (cj0) * (px))) + + (((IkReal(-0.924698002368864)) * (pp))))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x56 = IKasin(((IkReal(0.994304644497180)) + + (((IkReal(0.184939600473773)) * (py) * (sj0))) + + (((IkReal(0.184939600473773)) * (cj0) * (px))) + + (((IkReal(-0.924698002368864)) * (pp))))); + j2array[0] = ((IkReal(-2.96465459743209)) + (((IkReal(-1.00000000000000)) * (x56)))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + j2array[1] = ((IkReal(0.176938056157703)) + (x56)); + sj2array[1] = IKsin(j2array[1]); + cj2array[1] = IKcos(j2array[1]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + if (j2array[1] > IKPI) + { + j2array[1] -= IK2PI; + } + else if (j2array[1] < -IKPI) + { + j2array[1] += IK2PI; + } + j2valid[1] = true; + 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 dummyeval[1]; + IkReal gconst1; + IkReal x57 = ((IkReal(0.755000000000000)) * (cj2)); + IkReal x58 = ((py) * (sj0)); + IkReal x59 = ((cj0) * (px)); + IkReal x60 = ((IkReal(0.135000000000000)) * (sj2)); + gconst1 = IKsign(((((IkReal(0.705000000000000)) * (pz))) + + (((IkReal(-1.00000000000000)) * (x58) * (x60))) + + (((IkReal(0.0755000000000000)) * (cj2))) + + (((IkReal(-0.755000000000000)) * (pz) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x57) * (x58))) + + (((IkReal(-1.00000000000000)) * (x59) * (x60))) + + (((IkReal(-1.00000000000000)) * (x57) * (x59))) + + (((IkReal(0.0135000000000000)) * (sj2))) + + (((IkReal(0.135000000000000)) * (cj2) * (pz))))); + IkReal x61 = ((IkReal(10.0000000000000)) * (sj2)); + IkReal x62 = ((cj0) * (px)); + IkReal x63 = ((py) * (sj0)); + IkReal x64 = ((IkReal(55.9259259259259)) * (cj2)); + dummyeval[0] = ((((IkReal(52.2222222222222)) * (pz))) + + (((IkReal(-1.00000000000000)) * (x62) * (x64))) + (sj2) + + (((IkReal(10.0000000000000)) * (cj2) * (pz))) + + (((IkReal(-1.00000000000000)) * (x63) * (x64))) + + (((IkReal(-1.00000000000000)) * (x61) * (x62))) + + (((IkReal(-1.00000000000000)) * (x61) * (x63))) + + (((IkReal(5.59259259259259)) * (cj2))) + + (((IkReal(-55.9259259259259)) * (pz) * (sj2)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst0; + IkReal x65 = ((IkReal(0.755000000000000)) * (sj2)); + IkReal x66 = ((cj0) * (px)); + IkReal x67 = ((py) * (sj0)); + IkReal x68 = ((IkReal(0.135000000000000)) * (cj2)); + gconst0 = IKsign(((IkReal(0.0705000000000000)) + (((x65) * (x66))) + + (((IkReal(-0.705000000000000)) * (x67))) + (((x65) * (x67))) + + (((IkReal(-0.755000000000000)) * (cj2) * (pz))) + + (((IkReal(-0.705000000000000)) * (x66))) + + (((IkReal(-1.00000000000000)) * (x66) * (x68))) + + (((IkReal(-1.00000000000000)) * (x67) * (x68))) + + (((IkReal(0.0135000000000000)) * (cj2))) + + (((IkReal(-0.135000000000000)) * (pz) * (sj2))) + + (((IkReal(-0.0755000000000000)) * (sj2))))); + IkReal x69 = ((cj0) * (px)); + IkReal x70 = ((IkReal(10.0000000000000)) * (cj2)); + IkReal x71 = ((IkReal(55.9259259259259)) * (sj2)); + IkReal x72 = ((py) * (sj0)); + dummyeval[0] = + ((IkReal(5.22222222222222)) + + (((IkReal(-1.00000000000000)) * (x70) * (x72))) + (((x71) * (x72))) + + (((IkReal(-52.2222222222222)) * (x72))) + + (((IkReal(-10.0000000000000)) * (pz) * (sj2))) + (cj2) + + (((IkReal(-52.2222222222222)) * (x69))) + + (((IkReal(-1.00000000000000)) * (x69) * (x70))) + + (((IkReal(-5.59259259259259)) * (sj2))) + + (((IkReal(-55.9259259259259)) * (cj2) * (pz))) + (((x69) * (x71)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j1array[1], cj1array[1], sj1array[1]; + bool j1valid[1] = {false}; + _nj1 = 1; + IkReal x73 = (sj2) * (sj2); + IkReal x74 = (cj2) * (cj2); + IkReal x75 = ((cj2) * (sj2)); + IkReal x76 = ((IkReal(1.00000000000000)) * (pz)); + if (IKabs(((gconst0) * + (((IkReal(-0.497025000000000)) + + (((IkReal(-0.190350000000000)) * (cj2))) + ((pz) * (pz)) + + (((IkReal(1.06455000000000)) * (sj2))) + + (((IkReal(0.203850000000000)) * (x75))) + + (((IkReal(-0.570025000000000)) * (x73))) + + (((IkReal(-0.0182250000000000)) * (x74))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst0) * + (((((IkReal(0.101925000000000)) * (x73))) + + (((IkReal(-0.101925000000000)) * (x74))) + + (((IkReal(0.100000000000000)) * (pz))) + + (((IkReal(0.551800000000000)) * (x75))) + + (((IkReal(-1.00000000000000)) * (py) * (sj0) * (x76))) + + (((IkReal(-0.532275000000000)) * (cj2))) + + (((IkReal(-0.0951750000000000)) * (sj2))) + + (((IkReal(-1.00000000000000)) * (cj0) * (px) * (x76))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j1array[0] = IKatan2( + ((gconst0) * (((IkReal(-0.497025000000000)) + + (((IkReal(-0.190350000000000)) * (cj2))) + + ((pz) * (pz)) + (((IkReal(1.06455000000000)) * (sj2))) + + (((IkReal(0.203850000000000)) * (x75))) + + (((IkReal(-0.570025000000000)) * (x73))) + + (((IkReal(-0.0182250000000000)) * (x74)))))), + ((gconst0) * + (((((IkReal(0.101925000000000)) * (x73))) + + (((IkReal(-0.101925000000000)) * (x74))) + + (((IkReal(0.100000000000000)) * (pz))) + + (((IkReal(0.551800000000000)) * (x75))) + + (((IkReal(-1.00000000000000)) * (py) * (sj0) * (x76))) + + (((IkReal(-0.532275000000000)) * (cj2))) + + (((IkReal(-0.0951750000000000)) * (sj2))) + + (((IkReal(-1.00000000000000)) * (cj0) * (px) * (x76))))))); + 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 x77 = IKsin(j1); + IkReal x78 = IKcos(j1); + IkReal x79 = ((IkReal(0.135000000000000)) * (sj2)); + IkReal x80 = ((cj0) * (px)); + IkReal x81 = ((IkReal(0.755000000000000)) * (sj2)); + IkReal x82 = ((py) * (sj0)); + IkReal x83 = ((IkReal(0.755000000000000)) * (cj2)); + IkReal x84 = ((IkReal(0.135000000000000)) * (cj2)); + IkReal x85 = ((IkReal(0.135000000000000)) * (x78)); + IkReal x86 = ((IkReal(1.00000000000000)) * (x78)); + IkReal x87 = ((IkReal(1.41000000000000)) * (x77)); + IkReal x88 = ((pz) * (x78)); + evalcond[0] = + ((IkReal(-0.705000000000000)) + (((x77) * (x80))) + + (((x77) * (x82))) + (((IkReal(-0.100000000000000)) * (x77))) + + (((IkReal(-1.00000000000000)) * (x84))) + (x88) + (x81)); + evalcond[1] = ((((IkReal(0.100000000000000)) * (x78))) + + (((IkReal(-1.00000000000000)) * (x80) * (x86))) + (x79) + + (((IkReal(-1.00000000000000)) * (x82) * (x86))) + (x83) + + (((pz) * (x77)))); + evalcond[2] = + ((((IkReal(-0.705000000000000)) * (x78))) + + (((IkReal(-1.00000000000000)) * (x78) * (x84))) + + (((x77) * (x79))) + (pz) + (((x78) * (x81))) + (((x77) * (x83)))); + evalcond[3] = + ((IkReal(0.0812250000000000)) + + (((IkReal(-0.141000000000000)) * (x77))) + + (((IkReal(-1.00000000000000)) * (pp))) + (((x82) * (x87))) + + (((IkReal(0.200000000000000)) * (x82))) + + (((IkReal(1.41000000000000)) * (x88))) + (((x80) * (x87))) + + (((IkReal(0.200000000000000)) * (x80)))); + evalcond[4] = + ((IkReal(0.100000000000000)) + + (((IkReal(-1.00000000000000)) * (x82))) + (((x77) * (x84))) + + (((IkReal(-1.00000000000000)) * (x77) * (x81))) + + (((x78) * (x83))) + (((x78) * (x79))) + + (((IkReal(0.705000000000000)) * (x77))) + + (((IkReal(-1.00000000000000)) * (x80)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j1array[1], cj1array[1], sj1array[1]; + bool j1valid[1] = {false}; + _nj1 = 1; + IkReal x228 = (sj2) * (sj2); + IkReal x229 = (cj2) * (cj2); + IkReal x230 = ((cj2) * (sj2)); + if (IKabs(((gconst1) * (((((IkReal(0.551800000000000)) * (x230))) + + (((IkReal(0.101925000000000)) * (x228))) + + (((IkReal(-0.100000000000000)) * (pz))) + + (((IkReal(-0.101925000000000)) * (x229))) + + (((cj0) * (px) * (pz))) + + (((IkReal(-0.532275000000000)) * (cj2))) + + (((IkReal(-0.0951750000000000)) * (sj2))) + + (((py) * (pz) * (sj0))))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst1) * + (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) + + (((IkReal(-0.203850000000000)) * (x230))) + + (((IkReal(-0.570025000000000)) * (x229))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j1array[0] = IKatan2( + ((gconst1) * + (((((IkReal(0.551800000000000)) * (x230))) + + (((IkReal(0.101925000000000)) * (x228))) + + (((IkReal(-0.100000000000000)) * (pz))) + + (((IkReal(-0.101925000000000)) * (x229))) + (((cj0) * (px) * (pz))) + + (((IkReal(-0.532275000000000)) * (cj2))) + + (((IkReal(-0.0951750000000000)) * (sj2))) + (((py) * (pz) * (sj0)))))), + ((gconst1) * (((((IkReal(-0.0182250000000000)) * (x228))) + ((pz) * (pz)) + + (((IkReal(-0.203850000000000)) * (x230))) + + (((IkReal(-0.570025000000000)) * (x229))))))); + 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 x231 = IKsin(j1); + IkReal x232 = IKcos(j1); + IkReal x233 = ((IkReal(0.135000000000000)) * (sj2)); + IkReal x234 = ((cj0) * (px)); + IkReal x235 = ((IkReal(0.755000000000000)) * (sj2)); + IkReal x236 = ((py) * (sj0)); + IkReal x237 = ((IkReal(0.755000000000000)) * (cj2)); + IkReal x238 = ((IkReal(0.135000000000000)) * (cj2)); + IkReal x239 = ((IkReal(0.135000000000000)) * (x232)); + IkReal x240 = ((IkReal(1.00000000000000)) * (x232)); + IkReal x241 = ((IkReal(1.41000000000000)) * (x231)); + IkReal x242 = ((pz) * (x232)); + evalcond[0] = + ((IkReal(-0.705000000000000)) + + (((IkReal(-1.00000000000000)) * (x238))) + (x235) + + (((x231) * (x236))) + (x242) + + (((IkReal(-0.100000000000000)) * (x231))) + (((x231) * (x234)))); + evalcond[1] = + ((((IkReal(0.100000000000000)) * (x232))) + (((pz) * (x231))) + (x233) + + (x237) + (((IkReal(-1.00000000000000)) * (x234) * (x240))) + + (((IkReal(-1.00000000000000)) * (x236) * (x240)))); + evalcond[2] = + ((((IkReal(-0.705000000000000)) * (x232))) + (((x231) * (x237))) + + (((x231) * (x233))) + (pz) + (((x232) * (x235))) + + (((IkReal(-1.00000000000000)) * (x232) * (x238)))); + evalcond[3] = + ((IkReal(0.0812250000000000)) + (((x236) * (x241))) + + (((IkReal(0.200000000000000)) * (x236))) + + (((IkReal(0.200000000000000)) * (x234))) + + (((IkReal(-1.00000000000000)) * (pp))) + (((x234) * (x241))) + + (((IkReal(1.41000000000000)) * (x242))) + + (((IkReal(-0.141000000000000)) * (x231)))); + evalcond[4] = + ((IkReal(0.100000000000000)) + + (((IkReal(-1.00000000000000)) * (x231) * (x235))) + + (((IkReal(0.705000000000000)) * (x231))) + + (((IkReal(-1.00000000000000)) * (x236))) + (((x231) * (x238))) + + (((x232) * (x233))) + (((x232) * (x237))) + + (((IkReal(-1.00000000000000)) * (x234)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + return solutions.GetNumSolutions() > 0; + } + inline void rotationfunction0(IkSolutionListBase& solutions) + { + for (int rotationiter = 0; rotationiter < 1; ++rotationiter) + { + IkReal x89 = ((cj0) * (r00)); + IkReal x90 = ((cj0) * (r01)); + IkReal x91 = ((sj1) * (sj2)); + IkReal x92 = ((IkReal(1.00000000000000)) * (sj0)); + IkReal x93 = ((r10) * (sj0)); + IkReal x94 = ((IkReal(1.00000000000000)) * (cj2)); + IkReal x95 = ((r12) * (sj0)); + IkReal x96 = ((cj0) * (r02)); + IkReal x97 = ((r11) * (sj0)); + IkReal x98 = ((((IkReal(-1.00000000000000)) * (cj1) * (x94))) + (x91)); + IkReal x99 = ((((IkReal(-1.00000000000000)) * (x91))) + (((cj1) * (cj2)))); + IkReal x100 = ((cj0) * (x99)); + IkReal x101 = ((((IkReal(-1.00000000000000)) * (sj1) * (x94))) + + (((IkReal(-1.00000000000000)) * (cj1) * (sj2)))); + IkReal x102 = ((sj0) * (x101)); + new_r00 = ((((r20) * (x98))) + (((x101) * (x89))) + (((x101) * (x93)))); + new_r01 = ((((x101) * (x90))) + (((x101) * (x97))) + (((r21) * (x98)))); + new_r02 = ((((x101) * (x95))) + (((r22) * (x98))) + (((x101) * (x96)))); + new_r10 = ((((IkReal(-1.00000000000000)) * (r00) * (x92))) + (((cj0) * (r10)))); + new_r11 = ((((IkReal(-1.00000000000000)) * (r01) * (x92))) + (((cj0) * (r11)))); + new_r12 = ((((IkReal(-1.00000000000000)) * (r02) * (x92))) + (((cj0) * (r12)))); + new_r20 = ((((x93) * (x99))) + (((x89) * (x99))) + (((r20) * (x101)))); + new_r21 = ((((r21) * (x101))) + (((x97) * (x99))) + (((x90) * (x99)))); + new_r22 = ((((x96) * (x99))) + (((r22) * (x101))) + (((x95) * (x99)))); + { + 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 (std::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 dummyeval[1]; + IkReal gconst4; + gconst4 = IKsign(sj4); + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst2; + gconst2 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst3; + gconst3 = IKsign( + ((((new_r10) * (new_r12) * (sj4))) + (((new_r00) * (new_r02) * (sj4))))); + dummyeval[0] = + ((((new_r10) * (new_r12) * (sj4))) + (((new_r00) * (new_r02) * (sj4)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[7]; + IkReal x103 = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = x103; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = new_r20; + evalcond[5] = new_r21; + evalcond[6] = x103; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000) + { + { + IkReal j3array[2], cj3array[2], sj3array[2]; + bool j3valid[2] = {false}; + _nj3 = 2; + if (IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x104 = IKatan2(new_r02, new_r12); + j3array[0] = ((IkReal(-1.00000000000000)) * (x104)); + sj3array[0] = IKsin(j3array[0]); + cj3array[0] = IKcos(j3array[0]); + j3array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x104)))); + 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] = + ((((new_r12) * (IKcos(j3)))) + + (((IkReal(-1.00000000000000)) * (new_r02) * (IKsin(j3))))); + if (IKabs(evalcond[0]) > 0.000001) + { + continue; + } + } + + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs( + IKsqr( + ((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * (new_r01))))) + + IKsqr(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = + IKatan2(((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))), + ((((new_r10) * (sj3))) + (((cj3) * (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[4]; + IkReal x105 = IKsin(j5); + IkReal x106 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x107 = ((IkReal(1.00000000000000)) * (IKcos(j5))); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (x106))) + + (((cj3) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (x105)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x107))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x106)))); + evalcond[2] = + ((((new_r11) * (sj3))) + (x105) + (((cj3) * (new_r01)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (x107))) + + (((new_r10) * (sj3))) + (((cj3) * (new_r00)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + 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 + { + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(j4, IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[5] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[6] = ((IkReal(-1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000) + { + { + IkReal j3array[2], cj3array[2], sj3array[2]; + bool j3valid[2] = {false}; + _nj3 = 2; + if (IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x108 = IKatan2(new_r02, new_r12); + j3array[0] = ((IkReal(-1.00000000000000)) * (x108)); + sj3array[0] = IKsin(j3array[0]); + cj3array[0] = IKcos(j3array[0]); + j3array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x108)))); + 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] = + ((((new_r12) * (IKcos(j3)))) + + (((IkReal(-1.00000000000000)) * (new_r02) * (IKsin(j3))))); + if (IKabs(evalcond[0]) > 0.000001) + { + continue; + } + } + + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (sj3))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) + + IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * + (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * + (sj3))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((new_r11) * (sj3))) + (((cj3) * (new_r01)))), + ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (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[4]; + IkReal x109 = IKcos(j5); + IkReal x110 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x111 = ((IkReal(1.00000000000000)) * (IKsin(j5))); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (x110))) + + (((IkReal(-1.00000000000000)) * (x111))) + + (((cj3) * (new_r10)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x109))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x110))) + + (((cj3) * (new_r11)))); + evalcond[2] = ((((new_r11) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x111))) + + (((cj3) * (new_r01)))); + evalcond[3] = + ((((new_r10) * (sj3))) + (x109) + (((cj3) * (new_r00)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + 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 + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j3array[1], cj3array[1], sj3array[1]; + bool j3valid[1] = {false}; + _nj3 = 1; + IkReal x112 = ((IkReal(-1.00000000000000)) * (cj4) * (gconst3) * (new_r20)); + if (IKabs(((new_r12) * (x112))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((new_r02) * (x112))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j3array[0] = IKatan2(((new_r12) * (x112)), ((new_r02) * (x112))); + 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 x113 = IKsin(j3); + IkReal x114 = IKcos(j3); + IkReal x115 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x116 = ((sj4) * (x113)); + IkReal x117 = ((sj4) * (x114)); + IkReal x118 = ((new_r02) * (x114)); + IkReal x119 = ((new_r12) * (x113)); + evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r02) * (x113))) + + (((new_r12) * (x114)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x115))) + (x119) + (x118)); + evalcond[2] = ((((new_r00) * (x117))) + (((cj4) * (new_r20))) + + (((new_r10) * (x116)))); + evalcond[3] = ((((new_r11) * (x116))) + (((new_r01) * (x117))) + + (((cj4) * (new_r21)))); + evalcond[4] = ((IkReal(-1.00000000000000)) + (((new_r02) * (x117))) + + (((new_r12) * (x116))) + (((cj4) * (new_r22)))); + evalcond[5] = ((((cj4) * (x118))) + (((cj4) * (x119))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x115)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst5; + gconst5 = IKsign(sj4); + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[11]; + IkReal x120 = ((IkReal(-1.00000000000000)) + (new_r22)); + IkReal x121 = + ((((cj3) * (new_r12))) + + (((IkReal(-1.00000000000000)) * (new_r02) * (sj3)))); + IkReal x122 = + ((((new_r12) * (sj3))) + (((cj3) * (new_r02)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = x120; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x121; + evalcond[5] = x121; + evalcond[6] = x122; + evalcond[7] = new_r20; + evalcond[8] = new_r21; + evalcond[9] = x120; + evalcond[10] = x122; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * + (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * + (new_r01))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r10) * (sj3))) + + (((cj3) * (new_r00))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * + (new_r11) * (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * + (new_r01))))) + + IKsqr(((((new_r10) * (sj3))) + + (((cj3) * (new_r00))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (new_r11) * + (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * + (new_r01)))), + ((((new_r10) * (sj3))) + (((cj3) * (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[4]; + IkReal x123 = IKsin(j5); + IkReal x124 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x125 = + ((IkReal(1.00000000000000)) * (IKcos(j5))); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x123))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (x124))) + + (((cj3) * (new_r10)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (new_r01) * + (x124))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x125)))); + evalcond[2] = ((((new_r11) * (sj3))) + (x123) + + (((cj3) * (new_r01)))); + evalcond[3] = + ((((new_r10) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x125))) + + (((cj3) * (new_r00)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + 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 x126 = ((new_r12) * (sj3)); + IkReal x127 = ((IkReal(1.00000000000000)) * (new_r02)); + IkReal x128 = + ((((IkReal(-1.00000000000000)) * (sj3) * (x127))) + + (((cj3) * (new_r12)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(j4, IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x128; + evalcond[5] = x128; + evalcond[6] = ((x126) + (((cj3) * (new_r02)))); + evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[9] = + ((IkReal(-1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + evalcond[10] = + ((((IkReal(-1.00000000000000)) * (cj3) * (x127))) + + (((IkReal(-1.00000000000000)) * (x126)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((new_r11) * (sj3))) + + (((cj3) * (new_r01))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((IkReal(-1.00000000000000)) * (cj3) * + (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * + (sj3))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((new_r11) * (sj3))) + + (((cj3) * (new_r01))))) + + IKsqr(((((IkReal(-1.00000000000000)) * + (cj3) * (new_r00))) + + (((IkReal(-1.00000000000000)) * + (new_r10) * (sj3))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((new_r11) * (sj3))) + (((cj3) * (new_r01)))), + ((((IkReal(-1.00000000000000)) * (cj3) * + (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * + (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[4]; + IkReal x129 = IKcos(j5); + IkReal x130 = + ((IkReal(1.00000000000000)) * (sj3)); + IkReal x131 = + ((IkReal(1.00000000000000)) * (IKsin(j5))); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x131))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (x130))) + + (((cj3) * (new_r10)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x129))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (new_r01) * + (x130)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x131))) + + (((new_r11) * (sj3))) + + (((cj3) * (new_r01)))); + evalcond[3] = ((x129) + (((new_r10) * (sj3))) + + (((cj3) * (new_r00)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + 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 + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * + (sj3))) + + (((cj3) * (new_r10))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r20) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r00) * + (sj3))) + + (((cj3) * (new_r10))))) + + IKsqr(((IkReal(-1.00000000000000)) * (new_r20) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) + + (((cj3) * (new_r10)))), + ((IkReal(-1.00000000000000)) * (new_r20) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))); + 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 x132 = IKsin(j5); + IkReal x133 = IKcos(j5); + IkReal x134 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x135 = ((new_r11) * (sj3)); + IkReal x136 = ((new_r10) * (sj3)); + IkReal x137 = ((cj3) * (cj4)); + IkReal x138 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x139 = ((IkReal(1.00000000000000)) * (x133)); + IkReal x140 = ((IkReal(1.00000000000000)) * (x132)); + evalcond[0] = ((new_r20) + (((sj4) * (x133)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x132) * (x138))) + + (new_r21)); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x140))) + + (((cj3) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (x134)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x139))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (new_r01) * + (x134)))); + evalcond[4] = ((((cj4) * (x132))) + (x135) + + (((cj3) * (new_r01)))); + evalcond[5] = + ((x136) + + (((IkReal(-1.00000000000000)) * (cj4) * (x139))) + + (((cj3) * (new_r00)))); + evalcond[6] = + ((((cj4) * (x135))) + (((new_r01) * (x137))) + + (x132) + (((IkReal(-1.00000000000000)) * + (new_r21) * (x138)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x139))) + + (((IkReal(-1.00000000000000)) * (new_r20) * + (x138))) + + (((cj4) * (x136))) + (((new_r00) * (x137)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + 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; + if (IKabs( + ((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + + (((cj3) * (new_r11))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((new_r21) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) + + IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * + (sj3))) + + (((cj3) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30))), + ((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + + (((cj3) * (new_r11))))); + 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 x141 = IKsin(j5); + IkReal x142 = IKcos(j5); + IkReal x143 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x144 = ((new_r11) * (sj3)); + IkReal x145 = ((new_r10) * (sj3)); + IkReal x146 = ((cj3) * (cj4)); + IkReal x147 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x148 = ((IkReal(1.00000000000000)) * (x142)); + IkReal x149 = ((IkReal(1.00000000000000)) * (x141)); + evalcond[0] = ((((sj4) * (x142))) + (new_r20)); + evalcond[1] = + ((new_r21) + + (((IkReal(-1.00000000000000)) * (x141) * (x147)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (x143))) + + (((cj3) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (x149)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x148))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x143))) + + (((cj3) * (new_r11)))); + evalcond[4] = + ((((cj4) * (x141))) + (x144) + (((cj3) * (new_r01)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (cj4) * (x148))) + + (x145) + (((cj3) * (new_r00)))); + evalcond[6] = + ((((cj4) * (x144))) + (((new_r01) * (x146))) + (x141) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x147)))); + evalcond[7] = + ((((cj4) * (x145))) + + (((IkReal(-1.00000000000000)) * (x148))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x147))) + + (((new_r00) * (x146)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + 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; + if (IKabs(((gconst5) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = + IKatan2(((gconst5) * (new_r21)), + ((IkReal(-1.00000000000000)) * (gconst5) * (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[8]; + IkReal x150 = IKsin(j5); + IkReal x151 = IKcos(j5); + IkReal x152 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x153 = ((new_r11) * (sj3)); + IkReal x154 = ((new_r10) * (sj3)); + IkReal x155 = ((cj3) * (cj4)); + IkReal x156 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x157 = ((IkReal(1.00000000000000)) * (x151)); + IkReal x158 = ((IkReal(1.00000000000000)) * (x150)); + evalcond[0] = ((((sj4) * (x151))) + (new_r20)); + evalcond[1] = + ((new_r21) + + (((IkReal(-1.00000000000000)) * (x150) * (x156)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (x152))) + + (((IkReal(-1.00000000000000)) * (x158))) + + (((cj3) * (new_r10)))); + evalcond[3] = + ((((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x157))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x152)))); + evalcond[4] = + ((x153) + (((cj4) * (x150))) + (((cj3) * (new_r01)))); + evalcond[5] = + ((x154) + (((IkReal(-1.00000000000000)) * (cj4) * (x157))) + + (((cj3) * (new_r00)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (new_r21) * (x156))) + + (x150) + (((new_r01) * (x155))) + (((cj4) * (x153)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x157))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x156))) + + (((cj4) * (x154))) + (((new_r00) * (x155)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + 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 x159 = ((gconst2) * (sj4)); + if (IKabs(((new_r12) * (x159))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((new_r02) * (x159))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j3array[0] = IKatan2(((new_r12) * (x159)), ((new_r02) * (x159))); + 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 x160 = IKsin(j3); + IkReal x161 = IKcos(j3); + IkReal x162 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x163 = ((sj4) * (x160)); + IkReal x164 = ((sj4) * (x161)); + IkReal x165 = ((new_r02) * (x161)); + IkReal x166 = ((new_r12) * (x160)); + evalcond[0] = ((((new_r12) * (x161))) + + (((IkReal(-1.00000000000000)) * (new_r02) * (x160)))); + evalcond[1] = ((x166) + (x165) + (((IkReal(-1.00000000000000)) * (x162)))); + evalcond[2] = ((((new_r00) * (x164))) + (((new_r10) * (x163))) + + (((cj4) * (new_r20)))); + evalcond[3] = ((((new_r01) * (x164))) + (((new_r11) * (x163))) + + (((cj4) * (new_r21)))); + evalcond[4] = ((IkReal(-1.00000000000000)) + (((new_r02) * (x164))) + + (((new_r12) * (x163))) + (((cj4) * (new_r22)))); + evalcond[5] = ((((cj4) * (x165))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x162))) + + (((cj4) * (x166)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst5; + gconst5 = IKsign(sj4); + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj4; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[11]; + IkReal x167 = ((IkReal(-1.00000000000000)) + (new_r22)); + IkReal x168 = + ((((cj3) * (new_r12))) + + (((IkReal(-1.00000000000000)) * (new_r02) * (sj3)))); + IkReal x169 = ((((new_r12) * (sj3))) + (((cj3) * (new_r02)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = x167; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x168; + evalcond[5] = x168; + evalcond[6] = x169; + evalcond[7] = new_r20; + evalcond[8] = new_r21; + evalcond[9] = x167; + evalcond[10] = x169; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (new_r11) * + (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * + (new_r01))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r10) * (sj3))) + (((cj3) * (new_r00))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r11) * + (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * + (new_r01))))) + + IKsqr(((((new_r10) * (sj3))) + + (((cj3) * (new_r00))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (new_r11) * (sj3))) + + (((IkReal(-1.00000000000000)) * (cj3) * (new_r01)))), + ((((new_r10) * (sj3))) + (((cj3) * (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[4]; + IkReal x170 = IKsin(j5); + IkReal x171 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x172 = + ((IkReal(1.00000000000000)) * (IKcos(j5))); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x170))) + + (((cj3) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (x171)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (new_r01) * + (x171))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x172)))); + evalcond[2] = ((x170) + (((new_r11) * (sj3))) + + (((cj3) * (new_r01)))); + evalcond[3] = + ((((new_r10) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x172))) + + (((cj3) * (new_r00)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + 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 x173 = ((new_r12) * (sj3)); + IkReal x174 = ((IkReal(1.00000000000000)) * (new_r02)); + IkReal x175 = + ((((cj3) * (new_r12))) + + (((IkReal(-1.00000000000000)) * (sj3) * (x174)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(j4, IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x175; + evalcond[5] = x175; + evalcond[6] = ((x173) + (((cj3) * (new_r02)))); + evalcond[7] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[9] = ((IkReal(-1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + evalcond[10] = + ((((IkReal(-1.00000000000000)) * (x173))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x174)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs( + ((((new_r11) * (sj3))) + (((cj3) * (new_r01))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((IkReal(-1.00000000000000)) * (cj3) * + (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * + (sj3))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((new_r11) * (sj3))) + + (((cj3) * (new_r01))))) + + IKsqr(((((IkReal(-1.00000000000000)) * (cj3) * + (new_r00))) + + (((IkReal(-1.00000000000000)) * + (new_r10) * (sj3))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((new_r11) * (sj3))) + (((cj3) * (new_r01)))), + ((((IkReal(-1.00000000000000)) * (cj3) * (new_r00))) + + (((IkReal(-1.00000000000000)) * (new_r10) * + (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[4]; + IkReal x176 = IKcos(j5); + IkReal x177 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x178 = + ((IkReal(1.00000000000000)) * (IKsin(j5))); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * + (x177))) + + (((IkReal(-1.00000000000000)) * (x178))) + + (((cj3) * (new_r10)))); + evalcond[1] = + ((((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (new_r01) * + (x177))) + + (((IkReal(-1.00000000000000)) * (x176)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x178))) + + (((new_r11) * (sj3))) + (((cj3) * (new_r01)))); + evalcond[3] = ((x176) + (((new_r10) * (sj3))) + + (((cj3) * (new_r00)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + 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 + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) + + (((cj3) * (new_r10))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r20) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (new_r00) * + (sj3))) + + (((cj3) * (new_r10))))) + + IKsqr(((IkReal(-1.00000000000000)) * (new_r20) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (new_r00) * (sj3))) + + (((cj3) * (new_r10)))), + ((IkReal(-1.00000000000000)) * (new_r20) * + (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))); + 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 x179 = IKsin(j5); + IkReal x180 = IKcos(j5); + IkReal x181 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x182 = ((new_r11) * (sj3)); + IkReal x183 = ((new_r10) * (sj3)); + IkReal x184 = ((cj3) * (cj4)); + IkReal x185 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x186 = ((IkReal(1.00000000000000)) * (x180)); + IkReal x187 = ((IkReal(1.00000000000000)) * (x179)); + evalcond[0] = ((((sj4) * (x180))) + (new_r20)); + evalcond[1] = + ((new_r21) + + (((IkReal(-1.00000000000000)) * (x179) * (x185)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (x181))) + + (((IkReal(-1.00000000000000)) * (x187))) + + (((cj3) * (new_r10)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (new_r01) * (x181))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x186)))); + evalcond[4] = + ((x182) + (((cj3) * (new_r01))) + (((cj4) * (x179)))); + evalcond[5] = + ((x183) + + (((IkReal(-1.00000000000000)) * (cj4) * (x186))) + + (((cj3) * (new_r00)))); + evalcond[6] = + ((x179) + (((cj4) * (x182))) + (((new_r01) * (x184))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x185)))); + evalcond[7] = + ((((new_r00) * (x184))) + (((cj4) * (x183))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x185))) + + (((IkReal(-1.00000000000000)) * (x186)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + 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; + if (IKabs(((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + + (((cj3) * (new_r11))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs( + IKsqr( + ((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30)))) + + IKsqr(((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + + (((cj3) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j5array[0] = + IKatan2(((new_r21) * (((IKabs(sj4) != 0) ? ((IkReal)1 / (sj4)) + : (IkReal)1.0e30))), + ((((IkReal(-1.00000000000000)) * (new_r01) * (sj3))) + + (((cj3) * (new_r11))))); + 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 x188 = IKsin(j5); + IkReal x189 = IKcos(j5); + IkReal x190 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x191 = ((new_r11) * (sj3)); + IkReal x192 = ((new_r10) * (sj3)); + IkReal x193 = ((cj3) * (cj4)); + IkReal x194 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x195 = ((IkReal(1.00000000000000)) * (x189)); + IkReal x196 = ((IkReal(1.00000000000000)) * (x188)); + evalcond[0] = ((new_r20) + (((sj4) * (x189)))); + evalcond[1] = + ((new_r21) + + (((IkReal(-1.00000000000000)) * (x188) * (x194)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x196))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (x190))) + + (((cj3) * (new_r10)))); + evalcond[3] = + ((((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x190))) + + (((IkReal(-1.00000000000000)) * (x195)))); + evalcond[4] = + ((((cj4) * (x188))) + (x191) + (((cj3) * (new_r01)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (cj4) * (x195))) + (x192) + + (((cj3) * (new_r00)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (new_r21) * (x194))) + + (((cj4) * (x191))) + (x188) + (((new_r01) * (x193)))); + evalcond[7] = + ((((new_r00) * (x193))) + + (((IkReal(-1.00000000000000)) * (x195))) + + (((cj4) * (x192))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x194)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + 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; + if (IKabs(((gconst5) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst5) * (new_r20))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = + IKatan2(((gconst5) * (new_r21)), + ((IkReal(-1.00000000000000)) * (gconst5) * (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[8]; + IkReal x197 = IKsin(j5); + IkReal x198 = IKcos(j5); + IkReal x199 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x200 = ((new_r11) * (sj3)); + IkReal x201 = ((new_r10) * (sj3)); + IkReal x202 = ((cj3) * (cj4)); + IkReal x203 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x204 = ((IkReal(1.00000000000000)) * (x198)); + IkReal x205 = ((IkReal(1.00000000000000)) * (x197)); + evalcond[0] = ((new_r20) + (((sj4) * (x198)))); + evalcond[1] = + ((new_r21) + (((IkReal(-1.00000000000000)) * (x197) * (x203)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x205))) + + (((cj3) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (x199)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x204))) + + (((cj3) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x199)))); + evalcond[4] = ((((cj4) * (x197))) + (x200) + (((cj3) * (new_r01)))); + evalcond[5] = + ((x201) + (((IkReal(-1.00000000000000)) * (cj4) * (x204))) + + (((cj3) * (new_r00)))); + evalcond[6] = + ((((new_r01) * (x202))) + (((cj4) * (x200))) + (x197) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x203)))); + evalcond[7] = + ((((cj4) * (x201))) + (((new_r00) * (x202))) + + (((IkReal(-1.00000000000000)) * (x204))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x203)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001) + { + 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; + if (IKabs(((gconst4) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst4) * (new_r20))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2(((gconst4) * (new_r21)), + ((IkReal(-1.00000000000000)) * (gconst4) * (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[2]; + evalcond[0] = ((new_r20) + (((sj4) * (IKcos(j5))))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (sj4) * (IKsin(j5)))) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst6; + gconst6 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst7; + gconst7 = IKsign(((((IkReal(-1.00000000000000)) * (new_r01) * (new_r02))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (new_r12))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * (new_r01) * (new_r02))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (new_r12)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j3array[1], cj3array[1], sj3array[1]; + bool j3valid[1] = {false}; + _nj3 = 1; + IkReal x206 = ((cj4) * (gconst7) * (sj5)); + if (IKabs(((new_r12) * (x206))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((new_r02) * (x206))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j3array[0] = IKatan2(((new_r12) * (x206)), ((new_r02) * (x206))); + 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[12]; + IkReal x207 = IKsin(j3); + IkReal x208 = IKcos(j3); + IkReal x209 = ((IkReal(1.00000000000000)) * (cj5)); + IkReal x210 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x211 = ((cj4) * (x208)); + IkReal x212 = ((sj4) * (x208)); + IkReal x213 = ((cj4) * (x207)); + IkReal x214 = ((new_r11) * (x207)); + IkReal x215 = ((sj4) * (x207)); + IkReal x216 = ((IkReal(1.00000000000000)) * (x207)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (new_r02) * (x216))) + + (((new_r12) * (x208)))); + evalcond[1] = ((((IkReal(-1.00000000000000)) * (x210))) + + (((new_r12) * (x207))) + (((new_r02) * (x208)))); + evalcond[2] = + ((((new_r10) * (x208))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (x216))) + + (((IkReal(-1.00000000000000)) * (sj5)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (new_r01) * (x216))) + + (((IkReal(-1.00000000000000)) * (x209))) + + (((new_r11) * (x208)))); + evalcond[4] = ((((new_r01) * (x208))) + (x214) + (((cj4) * (sj5)))); + evalcond[5] = ((((new_r10) * (x207))) + (((new_r00) * (x208))) + + (((IkReal(-1.00000000000000)) * (cj4) * (x209)))); + evalcond[6] = ((((new_r00) * (x212))) + (((new_r10) * (x215))) + + (((cj4) * (new_r20)))); + evalcond[7] = ((((cj4) * (new_r21))) + (((new_r01) * (x212))) + + (((sj4) * (x214)))); + evalcond[8] = ((IkReal(-1.00000000000000)) + (((cj4) * (new_r22))) + + (((new_r02) * (x212))) + (((new_r12) * (x215)))); + evalcond[9] = + ((((new_r12) * (x213))) + (((new_r02) * (x211))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x210)))); + evalcond[10] = + ((((new_r11) * (x213))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x210))) + (sj5) + + (((new_r01) * (x211)))); + evalcond[11] = + ((((IkReal(-1.00000000000000)) * (x209))) + + (((new_r00) * (x211))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x210))) + + (((new_r10) * (x213)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001 || + IKabs(evalcond[8]) > 0.000001 || + IKabs(evalcond[9]) > 0.000001 || + IKabs(evalcond[10]) > 0.000001 || + IKabs(evalcond[11]) > 0.000001) + { + 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 x217 = ((gconst6) * (sj4)); + if (IKabs(((new_r12) * (x217))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((new_r02) * (x217))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j3array[0] = IKatan2(((new_r12) * (x217)), ((new_r02) * (x217))); + 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[12]; + IkReal x218 = IKsin(j3); + IkReal x219 = IKcos(j3); + IkReal x220 = ((IkReal(1.00000000000000)) * (cj5)); + IkReal x221 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x222 = ((cj4) * (x219)); + IkReal x223 = ((sj4) * (x219)); + IkReal x224 = ((cj4) * (x218)); + IkReal x225 = ((new_r11) * (x218)); + IkReal x226 = ((sj4) * (x218)); + IkReal x227 = ((IkReal(1.00000000000000)) * (x218)); + evalcond[0] = ((((new_r12) * (x219))) + + (((IkReal(-1.00000000000000)) * (new_r02) * (x227)))); + evalcond[1] = ((((IkReal(-1.00000000000000)) * (x221))) + + (((new_r12) * (x218))) + (((new_r02) * (x219)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (x227))) + + (((new_r10) * (x219))) + (((IkReal(-1.00000000000000)) * (sj5)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x220))) + (((new_r11) * (x219))) + + (((IkReal(-1.00000000000000)) * (new_r01) * (x227)))); + evalcond[4] = ((((new_r01) * (x219))) + (x225) + (((cj4) * (sj5)))); + evalcond[5] = ((((new_r00) * (x219))) + (((new_r10) * (x218))) + + (((IkReal(-1.00000000000000)) * (cj4) * (x220)))); + evalcond[6] = ((((new_r00) * (x223))) + (((cj4) * (new_r20))) + + (((new_r10) * (x226)))); + evalcond[7] = ((((sj4) * (x225))) + (((cj4) * (new_r21))) + + (((new_r01) * (x223)))); + evalcond[8] = ((IkReal(-1.00000000000000)) + (((new_r12) * (x226))) + + (((new_r02) * (x223))) + (((cj4) * (new_r22)))); + evalcond[9] = ((((IkReal(-1.00000000000000)) * (new_r22) * (x221))) + + (((new_r12) * (x224))) + (((new_r02) * (x222)))); + evalcond[10] = + ((((new_r11) * (x224))) + (((new_r01) * (x222))) + (sj5) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x221)))); + evalcond[11] = + ((((IkReal(-1.00000000000000)) * (x220))) + (((new_r10) * (x224))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x221))) + + (((new_r00) * (x222)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || + IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || + IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001) + { + 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); + } + } + } + } + } + } + } + } + } + } + } + } + } +}; +/// 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) { -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(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[4]; -IkReal x109=IKcos(j5); -IkReal x110=((IkReal(1.00000000000000))*(sj3)); -IkReal x111=((IkReal(1.00000000000000))*(IKsin(j5))); -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x110)))+(((IkReal(-1.00000000000000))*(x111)))+(((cj3)*(new_r10)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x109)))+(((IkReal(-1.00000000000000))*(new_r01)*(x110)))+(((cj3)*(new_r11)))); -evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x111)))+(((cj3)*(new_r01)))); -evalcond[3]=((((new_r10)*(sj3)))+(x109)+(((cj3)*(new_r00)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} + IKSolver solver; + return solver.ComputeIk(eetrans, eerot, pfree, solutions); } +IKFAST_API const char* GetKinematicsHash() { -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); -} -} -} -} + return ""; } -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} +IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } -} else -{ -{ -IkReal j3array[1], cj3array[1], sj3array[1]; -bool j3valid[1]={false}; -_nj3 = 1; -IkReal x112=((IkReal(-1.00000000000000))*(cj4)*(gconst3)*(new_r20)); -if( IKabs(((new_r12)*(x112))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x112))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j3array[0]=IKatan2(((new_r12)*(x112)), ((new_r02)*(x112))); -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 x113=IKsin(j3); -IkReal x114=IKcos(j3); -IkReal x115=((IkReal(1.00000000000000))*(sj4)); -IkReal x116=((sj4)*(x113)); -IkReal x117=((sj4)*(x114)); -IkReal x118=((new_r02)*(x114)); -IkReal x119=((new_r12)*(x113)); -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x113)))+(((new_r12)*(x114)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x115)))+(x119)+(x118)); -evalcond[2]=((((new_r00)*(x117)))+(((cj4)*(new_r20)))+(((new_r10)*(x116)))); -evalcond[3]=((((new_r11)*(x116)))+(((new_r01)*(x117)))+(((cj4)*(new_r21)))); -evalcond[4]=((IkReal(-1.00000000000000))+(((new_r02)*(x117)))+(((new_r12)*(x116)))+(((cj4)*(new_r22)))); -evalcond[5]=((((cj4)*(x118)))+(((cj4)*(x119)))+(((IkReal(-1.00000000000000))*(new_r22)*(x115)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} +#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) { -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(sj4); -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x120=((IkReal(-1.00000000000000))+(new_r22)); -IkReal x121=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(sj3)))); -IkReal x122=((((new_r12)*(sj3)))+(((cj3)*(new_r02)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=x120; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x121; -evalcond[5]=x121; -evalcond[6]=x122; -evalcond[7]=new_r20; -evalcond[8]=new_r21; -evalcond[9]=x120; -evalcond[10]=x122; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(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[4]; -IkReal x123=IKsin(j5); -IkReal x124=((IkReal(1.00000000000000))*(sj3)); -IkReal x125=((IkReal(1.00000000000000))*(IKcos(j5))); -evalcond[0]=((((IkReal(-1.00000000000000))*(x123)))+(((IkReal(-1.00000000000000))*(new_r00)*(x124)))+(((cj3)*(new_r10)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x124)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x125)))); -evalcond[2]=((((new_r11)*(sj3)))+(x123)+(((cj3)*(new_r01)))); -evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x125)))+(((cj3)*(new_r00)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -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 x126=((new_r12)*(sj3)); -IkReal x127=((IkReal(1.00000000000000))*(new_r02)); -IkReal x128=((((IkReal(-1.00000000000000))*(sj3)*(x127)))+(((cj3)*(new_r12)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x128; -evalcond[5]=x128; -evalcond[6]=((x126)+(((cj3)*(new_r02)))); -evalcond[7]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[8]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[9]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -evalcond[10]=((((IkReal(-1.00000000000000))*(cj3)*(x127)))+(((IkReal(-1.00000000000000))*(x126)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(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[4]; -IkReal x129=IKcos(j5); -IkReal x130=((IkReal(1.00000000000000))*(sj3)); -IkReal x131=((IkReal(1.00000000000000))*(IKsin(j5))); -evalcond[0]=((((IkReal(-1.00000000000000))*(x131)))+(((IkReal(-1.00000000000000))*(new_r00)*(x130)))+(((cj3)*(new_r10)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x129)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x130)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x131)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01)))); -evalcond[3]=((x129)+(((new_r10)*(sj3)))+(((cj3)*(new_r00)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -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 -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))), ((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))); -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 x132=IKsin(j5); -IkReal x133=IKcos(j5); -IkReal x134=((IkReal(1.00000000000000))*(sj3)); -IkReal x135=((new_r11)*(sj3)); -IkReal x136=((new_r10)*(sj3)); -IkReal x137=((cj3)*(cj4)); -IkReal x138=((IkReal(1.00000000000000))*(sj4)); -IkReal x139=((IkReal(1.00000000000000))*(x133)); -IkReal x140=((IkReal(1.00000000000000))*(x132)); -evalcond[0]=((new_r20)+(((sj4)*(x133)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x132)*(x138)))+(new_r21)); -evalcond[2]=((((IkReal(-1.00000000000000))*(x140)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x134)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x139)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x134)))); -evalcond[4]=((((cj4)*(x132)))+(x135)+(((cj3)*(new_r01)))); -evalcond[5]=((x136)+(((IkReal(-1.00000000000000))*(cj4)*(x139)))+(((cj3)*(new_r00)))); -evalcond[6]=((((cj4)*(x135)))+(((new_r01)*(x137)))+(x132)+(((IkReal(-1.00000000000000))*(new_r21)*(x138)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x139)))+(((IkReal(-1.00000000000000))*(new_r20)*(x138)))+(((cj4)*(x136)))+(((new_r00)*(x137)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -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; -if( IKabs(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))); -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 x141=IKsin(j5); -IkReal x142=IKcos(j5); -IkReal x143=((IkReal(1.00000000000000))*(sj3)); -IkReal x144=((new_r11)*(sj3)); -IkReal x145=((new_r10)*(sj3)); -IkReal x146=((cj3)*(cj4)); -IkReal x147=((IkReal(1.00000000000000))*(sj4)); -IkReal x148=((IkReal(1.00000000000000))*(x142)); -IkReal x149=((IkReal(1.00000000000000))*(x141)); -evalcond[0]=((((sj4)*(x142)))+(new_r20)); -evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x141)*(x147)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x143)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(x149)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x148)))+(((IkReal(-1.00000000000000))*(new_r01)*(x143)))+(((cj3)*(new_r11)))); -evalcond[4]=((((cj4)*(x141)))+(x144)+(((cj3)*(new_r01)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x148)))+(x145)+(((cj3)*(new_r00)))); -evalcond[6]=((((cj4)*(x144)))+(((new_r01)*(x146)))+(x141)+(((IkReal(-1.00000000000000))*(new_r21)*(x147)))); -evalcond[7]=((((cj4)*(x145)))+(((IkReal(-1.00000000000000))*(x148)))+(((IkReal(-1.00000000000000))*(new_r20)*(x147)))+(((new_r00)*(x146)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -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; -if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst5)*(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[8]; -IkReal x150=IKsin(j5); -IkReal x151=IKcos(j5); -IkReal x152=((IkReal(1.00000000000000))*(sj3)); -IkReal x153=((new_r11)*(sj3)); -IkReal x154=((new_r10)*(sj3)); -IkReal x155=((cj3)*(cj4)); -IkReal x156=((IkReal(1.00000000000000))*(sj4)); -IkReal x157=((IkReal(1.00000000000000))*(x151)); -IkReal x158=((IkReal(1.00000000000000))*(x150)); -evalcond[0]=((((sj4)*(x151)))+(new_r20)); -evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x150)*(x156)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x152)))+(((IkReal(-1.00000000000000))*(x158)))+(((cj3)*(new_r10)))); -evalcond[3]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x157)))+(((IkReal(-1.00000000000000))*(new_r01)*(x152)))); -evalcond[4]=((x153)+(((cj4)*(x150)))+(((cj3)*(new_r01)))); -evalcond[5]=((x154)+(((IkReal(-1.00000000000000))*(cj4)*(x157)))+(((cj3)*(new_r00)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(new_r21)*(x156)))+(x150)+(((new_r01)*(x155)))+(((cj4)*(x153)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x157)))+(((IkReal(-1.00000000000000))*(new_r20)*(x156)))+(((cj4)*(x154)))+(((new_r00)*(x155)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -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 x159=((gconst2)*(sj4)); -if( IKabs(((new_r12)*(x159))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x159))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j3array[0]=IKatan2(((new_r12)*(x159)), ((new_r02)*(x159))); -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 x160=IKsin(j3); -IkReal x161=IKcos(j3); -IkReal x162=((IkReal(1.00000000000000))*(sj4)); -IkReal x163=((sj4)*(x160)); -IkReal x164=((sj4)*(x161)); -IkReal x165=((new_r02)*(x161)); -IkReal x166=((new_r12)*(x160)); -evalcond[0]=((((new_r12)*(x161)))+(((IkReal(-1.00000000000000))*(new_r02)*(x160)))); -evalcond[1]=((x166)+(x165)+(((IkReal(-1.00000000000000))*(x162)))); -evalcond[2]=((((new_r00)*(x164)))+(((new_r10)*(x163)))+(((cj4)*(new_r20)))); -evalcond[3]=((((new_r01)*(x164)))+(((new_r11)*(x163)))+(((cj4)*(new_r21)))); -evalcond[4]=((IkReal(-1.00000000000000))+(((new_r02)*(x164)))+(((new_r12)*(x163)))+(((cj4)*(new_r22)))); -evalcond[5]=((((cj4)*(x165)))+(((IkReal(-1.00000000000000))*(new_r22)*(x162)))+(((cj4)*(x166)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(sj4); -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj4; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x167=((IkReal(-1.00000000000000))+(new_r22)); -IkReal x168=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(sj3)))); -IkReal x169=((((new_r12)*(sj3)))+(((cj3)*(new_r02)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=x167; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x168; -evalcond[5]=x168; -evalcond[6]=x169; -evalcond[7]=new_r20; -evalcond[8]=new_r21; -evalcond[9]=x167; -evalcond[10]=x169; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r10)*(sj3)))+(((cj3)*(new_r00))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))))+IKsqr(((((new_r10)*(sj3)))+(((cj3)*(new_r00)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(cj3)*(new_r01)))), ((((new_r10)*(sj3)))+(((cj3)*(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[4]; -IkReal x170=IKsin(j5); -IkReal x171=((IkReal(1.00000000000000))*(sj3)); -IkReal x172=((IkReal(1.00000000000000))*(IKcos(j5))); -evalcond[0]=((((IkReal(-1.00000000000000))*(x170)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x171)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(new_r01)*(x171)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x172)))); -evalcond[2]=((x170)+(((new_r11)*(sj3)))+(((cj3)*(new_r01)))); -evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x172)))+(((cj3)*(new_r00)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -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 x173=((new_r12)*(sj3)); -IkReal x174=((IkReal(1.00000000000000))*(new_r02)); -IkReal x175=((((cj3)*(new_r12)))+(((IkReal(-1.00000000000000))*(sj3)*(x174)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x175; -evalcond[5]=x175; -evalcond[6]=((x173)+(((cj3)*(new_r02)))); -evalcond[7]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[8]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[9]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -evalcond[10]=((((IkReal(-1.00000000000000))*(x173)))+(((IkReal(-1.00000000000000))*(cj3)*(x174)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((new_r11)*(sj3)))+(((cj3)*(new_r01))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))))+IKsqr(((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj3)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((new_r11)*(sj3)))+(((cj3)*(new_r01)))), ((((IkReal(-1.00000000000000))*(cj3)*(new_r00)))+(((IkReal(-1.00000000000000))*(new_r10)*(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[4]; -IkReal x176=IKcos(j5); -IkReal x177=((IkReal(1.00000000000000))*(sj3)); -IkReal x178=((IkReal(1.00000000000000))*(IKsin(j5))); -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r00)*(x177)))+(((IkReal(-1.00000000000000))*(x178)))+(((cj3)*(new_r10)))); -evalcond[1]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x177)))+(((IkReal(-1.00000000000000))*(x176)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x178)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01)))); -evalcond[3]=((x176)+(((new_r10)*(sj3)))+(((cj3)*(new_r00)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -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 -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))), ((IkReal(-1.00000000000000))*(new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))); -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 x179=IKsin(j5); -IkReal x180=IKcos(j5); -IkReal x181=((IkReal(1.00000000000000))*(sj3)); -IkReal x182=((new_r11)*(sj3)); -IkReal x183=((new_r10)*(sj3)); -IkReal x184=((cj3)*(cj4)); -IkReal x185=((IkReal(1.00000000000000))*(sj4)); -IkReal x186=((IkReal(1.00000000000000))*(x180)); -IkReal x187=((IkReal(1.00000000000000))*(x179)); -evalcond[0]=((((sj4)*(x180)))+(new_r20)); -evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x179)*(x185)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x181)))+(((IkReal(-1.00000000000000))*(x187)))+(((cj3)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x181)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(x186)))); -evalcond[4]=((x182)+(((cj3)*(new_r01)))+(((cj4)*(x179)))); -evalcond[5]=((x183)+(((IkReal(-1.00000000000000))*(cj4)*(x186)))+(((cj3)*(new_r00)))); -evalcond[6]=((x179)+(((cj4)*(x182)))+(((new_r01)*(x184)))+(((IkReal(-1.00000000000000))*(new_r21)*(x185)))); -evalcond[7]=((((new_r00)*(x184)))+(((cj4)*(x183)))+(((IkReal(-1.00000000000000))*(new_r20)*(x185)))+(((IkReal(-1.00000000000000))*(x186)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -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; -if( IKabs(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j5array[0]=IKatan2(((new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))); -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 x188=IKsin(j5); -IkReal x189=IKcos(j5); -IkReal x190=((IkReal(1.00000000000000))*(sj3)); -IkReal x191=((new_r11)*(sj3)); -IkReal x192=((new_r10)*(sj3)); -IkReal x193=((cj3)*(cj4)); -IkReal x194=((IkReal(1.00000000000000))*(sj4)); -IkReal x195=((IkReal(1.00000000000000))*(x189)); -IkReal x196=((IkReal(1.00000000000000))*(x188)); -evalcond[0]=((new_r20)+(((sj4)*(x189)))); -evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x188)*(x194)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x196)))+(((IkReal(-1.00000000000000))*(new_r00)*(x190)))+(((cj3)*(new_r10)))); -evalcond[3]=((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x190)))+(((IkReal(-1.00000000000000))*(x195)))); -evalcond[4]=((((cj4)*(x188)))+(x191)+(((cj3)*(new_r01)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x195)))+(x192)+(((cj3)*(new_r00)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(new_r21)*(x194)))+(((cj4)*(x191)))+(x188)+(((new_r01)*(x193)))); -evalcond[7]=((((new_r00)*(x193)))+(((IkReal(-1.00000000000000))*(x195)))+(((cj4)*(x192)))+(((IkReal(-1.00000000000000))*(new_r20)*(x194)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -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; -if( IKabs(((gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst5)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst5)*(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[8]; -IkReal x197=IKsin(j5); -IkReal x198=IKcos(j5); -IkReal x199=((IkReal(1.00000000000000))*(sj3)); -IkReal x200=((new_r11)*(sj3)); -IkReal x201=((new_r10)*(sj3)); -IkReal x202=((cj3)*(cj4)); -IkReal x203=((IkReal(1.00000000000000))*(sj4)); -IkReal x204=((IkReal(1.00000000000000))*(x198)); -IkReal x205=((IkReal(1.00000000000000))*(x197)); -evalcond[0]=((new_r20)+(((sj4)*(x198)))); -evalcond[1]=((new_r21)+(((IkReal(-1.00000000000000))*(x197)*(x203)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x205)))+(((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x199)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x204)))+(((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x199)))); -evalcond[4]=((((cj4)*(x197)))+(x200)+(((cj3)*(new_r01)))); -evalcond[5]=((x201)+(((IkReal(-1.00000000000000))*(cj4)*(x204)))+(((cj3)*(new_r00)))); -evalcond[6]=((((new_r01)*(x202)))+(((cj4)*(x200)))+(x197)+(((IkReal(-1.00000000000000))*(new_r21)*(x203)))); -evalcond[7]=((((cj4)*(x201)))+(((new_r00)*(x202)))+(((IkReal(-1.00000000000000))*(x204)))+(((IkReal(-1.00000000000000))*(new_r20)*(x203)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -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; -if( IKabs(((gconst4)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst4)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst4)*(new_r21)), ((IkReal(-1.00000000000000))*(gconst4)*(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[2]; -evalcond[0]=((new_r20)+(((sj4)*(IKcos(j5))))); -evalcond[1]=((((IkReal(-1.00000000000000))*(sj4)*(IKsin(j5))))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst6; -gconst6=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst7; -gconst7=IKsign(((((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r11)*(new_r12))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r01)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r11)*(new_r12)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j3array[1], cj3array[1], sj3array[1]; -bool j3valid[1]={false}; -_nj3 = 1; -IkReal x206=((cj4)*(gconst7)*(sj5)); -if( IKabs(((new_r12)*(x206))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x206))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j3array[0]=IKatan2(((new_r12)*(x206)), ((new_r02)*(x206))); -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[12]; -IkReal x207=IKsin(j3); -IkReal x208=IKcos(j3); -IkReal x209=((IkReal(1.00000000000000))*(cj5)); -IkReal x210=((IkReal(1.00000000000000))*(sj4)); -IkReal x211=((cj4)*(x208)); -IkReal x212=((sj4)*(x208)); -IkReal x213=((cj4)*(x207)); -IkReal x214=((new_r11)*(x207)); -IkReal x215=((sj4)*(x207)); -IkReal x216=((IkReal(1.00000000000000))*(x207)); -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x216)))+(((new_r12)*(x208)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x210)))+(((new_r12)*(x207)))+(((new_r02)*(x208)))); -evalcond[2]=((((new_r10)*(x208)))+(((IkReal(-1.00000000000000))*(new_r00)*(x216)))+(((IkReal(-1.00000000000000))*(sj5)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r01)*(x216)))+(((IkReal(-1.00000000000000))*(x209)))+(((new_r11)*(x208)))); -evalcond[4]=((((new_r01)*(x208)))+(x214)+(((cj4)*(sj5)))); -evalcond[5]=((((new_r10)*(x207)))+(((new_r00)*(x208)))+(((IkReal(-1.00000000000000))*(cj4)*(x209)))); -evalcond[6]=((((new_r00)*(x212)))+(((new_r10)*(x215)))+(((cj4)*(new_r20)))); -evalcond[7]=((((cj4)*(new_r21)))+(((new_r01)*(x212)))+(((sj4)*(x214)))); -evalcond[8]=((IkReal(-1.00000000000000))+(((cj4)*(new_r22)))+(((new_r02)*(x212)))+(((new_r12)*(x215)))); -evalcond[9]=((((new_r12)*(x213)))+(((new_r02)*(x211)))+(((IkReal(-1.00000000000000))*(new_r22)*(x210)))); -evalcond[10]=((((new_r11)*(x213)))+(((IkReal(-1.00000000000000))*(new_r21)*(x210)))+(sj5)+(((new_r01)*(x211)))); -evalcond[11]=((((IkReal(-1.00000000000000))*(x209)))+(((new_r00)*(x211)))+(((IkReal(-1.00000000000000))*(new_r20)*(x210)))+(((new_r10)*(x213)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 ) -{ -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 x217=((gconst6)*(sj4)); -if( IKabs(((new_r12)*(x217))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x217))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j3array[0]=IKatan2(((new_r12)*(x217)), ((new_r02)*(x217))); -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[12]; -IkReal x218=IKsin(j3); -IkReal x219=IKcos(j3); -IkReal x220=((IkReal(1.00000000000000))*(cj5)); -IkReal x221=((IkReal(1.00000000000000))*(sj4)); -IkReal x222=((cj4)*(x219)); -IkReal x223=((sj4)*(x219)); -IkReal x224=((cj4)*(x218)); -IkReal x225=((new_r11)*(x218)); -IkReal x226=((sj4)*(x218)); -IkReal x227=((IkReal(1.00000000000000))*(x218)); -evalcond[0]=((((new_r12)*(x219)))+(((IkReal(-1.00000000000000))*(new_r02)*(x227)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x221)))+(((new_r12)*(x218)))+(((new_r02)*(x219)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r00)*(x227)))+(((new_r10)*(x219)))+(((IkReal(-1.00000000000000))*(sj5)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x220)))+(((new_r11)*(x219)))+(((IkReal(-1.00000000000000))*(new_r01)*(x227)))); -evalcond[4]=((((new_r01)*(x219)))+(x225)+(((cj4)*(sj5)))); -evalcond[5]=((((new_r00)*(x219)))+(((new_r10)*(x218)))+(((IkReal(-1.00000000000000))*(cj4)*(x220)))); -evalcond[6]=((((new_r00)*(x223)))+(((cj4)*(new_r20)))+(((new_r10)*(x226)))); -evalcond[7]=((((sj4)*(x225)))+(((cj4)*(new_r21)))+(((new_r01)*(x223)))); -evalcond[8]=((IkReal(-1.00000000000000))+(((new_r12)*(x226)))+(((new_r02)*(x223)))+(((cj4)*(new_r22)))); -evalcond[9]=((((IkReal(-1.00000000000000))*(new_r22)*(x221)))+(((new_r12)*(x224)))+(((new_r02)*(x222)))); -evalcond[10]=((((new_r11)*(x224)))+(((new_r01)*(x222)))+(sj5)+(((IkReal(-1.00000000000000))*(new_r21)*(x221)))); -evalcond[11]=((((IkReal(-1.00000000000000))*(x220)))+(((new_r10)*(x224)))+(((IkReal(-1.00000000000000))*(new_r20)*(x221)))+(((new_r00)*(x222)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 ) -{ -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); -} -} -} - -} - -} -} -} - -} - -} -} -} -} -}}; - - -/// 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 const char* GetKinematicsHash() { return ""; } - -IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } - -#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; + if (argc != 12 + GetNumFreeParameters() + 1) + { + printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" + "Returns the ik solutions given the transformation of the end effector specified by\n" + "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" + "There are %d free parameters that have to be specified.\n\n", + GetNumFreeParameters()); + return 1; + } + + IkSolutionList solutions; + std::vector vfree(GetNumFreeParameters()); + IkReal eerot[9], eetrans[3]; + eerot[0] = atof(argv[1]); + eerot[1] = atof(argv[2]); + eerot[2] = atof(argv[3]); + eetrans[0] = atof(argv[4]); + eerot[3] = atof(argv[5]); + eerot[4] = atof(argv[6]); + eerot[5] = atof(argv[7]); + eetrans[1] = atof(argv[8]); + eerot[6] = atof(argv[9]); + eerot[7] = atof(argv[10]); + eerot[8] = atof(argv[11]); + eetrans[2] = atof(argv[12]); + for (std::size_t i = 0; i < vfree.size(); ++i) + vfree[i] = atof(argv[13 + i]); + bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + + if (!bSuccess) + { + fprintf(stderr, "Failed to get ik solution\n"); + return -1; + } + + printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions()); + std::vector solvalues(GetNumJoints()); + for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) + { + const IkSolutionBase& sol = solutions.GetSolution(i); + printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); + for (std::size_t j = 0; j < solvalues.size(); ++j) + printf("%.15f, ", solvalues[j]); + printf("\n"); + } + return 0; } #endif diff --git a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/ikfast.h b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/ikfast.h index 9a2a2f17..a8b9e9e8 100644 --- a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/ikfast.h +++ b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/include/irb2400_ikfast_manipulator_plugin/ikfast.h @@ -16,18 +16,23 @@ The ikfast inverse kinematics compiler is part of OpenRAVE. The file is divided into two sections: - - Common - the abstract classes section that all ikfast share regardless of their settings - - Library Specific - the library-specific definitions, which depends on the precision/settings that the library was compiled with + - Common - the abstract classes section that all ikfast share regardless of their + settings + - Library Specific - the library-specific definitions, which depends on the + precision/settings that the library was compiled with The defines are as follows, they are also used for the ikfast C++ class: - IKFAST_HEADER_COMMON - common classes - - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off + - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is + off - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility. - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY - - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected. + - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and + other invalid conditions are detected. - IKFAST_REAL - Use to force a custom real number type for IkReal. - - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded. + - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function + is excluded. */ #include @@ -40,224 +45,246 @@ /// should be the same as ikfast.__version__ #define IKFAST_VERSION 61 -namespace ikfast { +namespace ikfast +{ /// \brief holds the solution for a single dof -template -class IkSingleDOFSolutionBase +template class IkSingleDOFSolutionBase { public: - IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) { - indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; - } - T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset - signed char freeind; ///< if >= 0, mimics another joint - unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider - unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself - unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root + IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) + { + indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; + } + T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset + signed char freeind; ///< if >= 0, mimics another joint + unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider + unsigned char + maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself + unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it + ///came from. sometimes a solution can be repeated for different + ///indices. store at least another repeated root }; /// \brief The discrete solutions are returned in this structure. /// /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. -/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is: -template -class IkSolutionBase +/// Stores all these solutions in the form of free variables that the user has to set when querying +/// the solution. Its prototype is: +template class IkSolutionBase { public: - virtual ~IkSolutionBase() { - } - /// \brief gets a concrete solution - /// - /// \param[out] solution the result - /// \param[in] freevalues values for the free parameters \se GetFree - virtual void GetSolution(T* solution, const T* freevalues) const = 0; - - /// \brief std::vector version of \ref GetSolution - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } - - /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned - /// - /// \return vector of indices indicating the free parameters - virtual const std::vector& GetFree() const = 0; - - /// \brief the dof of the solution - virtual const int GetDOF() const = 0; + virtual ~IkSolutionBase() {} + /// \brief gets a concrete solution + /// + /// \param[out] solution the result + /// \param[in] freevalues values for the free parameters \se GetFree + virtual void GetSolution(T* solution, const T* freevalues) const = 0; + + /// \brief std::vector version of \ref GetSolution + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + } + + /// \brief Gets the indices of the configuration space that have to be preset before a full + /// solution can be returned + /// + /// \return vector of indices indicating the free parameters + virtual const std::vector& GetFree() const = 0; + + /// \brief the dof of the solution + virtual const int GetDOF() const = 0; }; /// \brief manages all the solutions -template -class IkSolutionListBase +template class IkSolutionListBase { public: - virtual ~IkSolutionListBase() { - } + virtual ~IkSolutionListBase() {} - /// \brief add one solution and return its index for later retrieval - /// - /// \param vinfos Solution data for each degree of freedom of the manipulator - /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; + /// \brief add one solution and return its index for later retrieval + /// + /// \param vinfos Solution data for each degree of freedom of the manipulator + /// \param vfree If the solution represents an infinite space, holds free parameters of the + /// solution that users can freely set. + virtual size_t AddSolution(const std::vector >& vinfos, + const std::vector& vfree) = 0; - /// \brief returns the solution pointer - virtual const IkSolutionBase& GetSolution(size_t index) const = 0; + /// \brief returns the solution pointer + virtual const IkSolutionBase& GetSolution(size_t index) const = 0; - /// \brief returns the number of solutions stored - virtual size_t GetNumSolutions() const = 0; + /// \brief returns the number of solutions stored + virtual size_t GetNumSolutions() const = 0; - /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated. - virtual void Clear() = 0; + /// \brief clears all current solutions, note that any memory addresses returned from \ref + /// GetSolution will be invalidated. + virtual void Clear() = 0; }; /// \brief holds function pointers for all the exported functions of ikfast -template -class IkFastFunctions +template class IkFastFunctions { public: - IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) { - } - virtual ~IkFastFunctions() { - } - typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); - ComputeIkFn _ComputeIk; - typedef void (*ComputeFkFn)(const T*, T*, T*); - ComputeFkFn _ComputeFk; - typedef int (*GetNumFreeParametersFn)(); - GetNumFreeParametersFn _GetNumFreeParameters; - typedef int* (*GetFreeParametersFn)(); - GetFreeParametersFn _GetFreeParameters; - typedef int (*GetNumJointsFn)(); - GetNumJointsFn _GetNumJoints; - typedef int (*GetIkRealSizeFn)(); - GetIkRealSizeFn _GetIkRealSize; - typedef const char* (*GetIkFastVersionFn)(); - GetIkFastVersionFn _GetIkFastVersion; - typedef int (*GetIkTypeFn)(); - GetIkTypeFn _GetIkType; - typedef const char* (*GetKinematicsHashFn)(); - GetKinematicsHashFn _GetKinematicsHash; + IkFastFunctions() + : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), + _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), + _GetKinematicsHash(NULL) + { + } + virtual ~IkFastFunctions() {} + typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); + ComputeIkFn _ComputeIk; + typedef void (*ComputeFkFn)(const T*, T*, T*); + ComputeFkFn _ComputeFk; + typedef int (*GetNumFreeParametersFn)(); + GetNumFreeParametersFn _GetNumFreeParameters; + typedef int* (*GetFreeParametersFn)(); + GetFreeParametersFn _GetFreeParameters; + typedef int (*GetNumJointsFn)(); + GetNumJointsFn _GetNumJoints; + typedef int (*GetIkRealSizeFn)(); + GetIkRealSizeFn _GetIkRealSize; + typedef const char* (*GetIkFastVersionFn)(); + GetIkFastVersionFn _GetIkFastVersion; + typedef int (*GetIkTypeFn)(); + GetIkTypeFn _GetIkType; + typedef const char* (*GetKinematicsHashFn)(); + GetKinematicsHashFn _GetKinematicsHash; }; // Implementations of the abstract classes, user doesn't need to use them /// \brief Default implementation of \ref IkSolutionBase -template -class IkSolution : public IkSolutionBase +template class IkSolution : public IkSolutionBase { public: - IkSolution(const std::vector >& vinfos, const std::vector& vfree) { - _vbasesol = vinfos; - _vfree = vfree; - } - - virtual void GetSolution(T* solution, const T* freevalues) const { - for(std::size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].freeind < 0 ) - solution[i] = _vbasesol[i].foffset; - else { - solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset; - if( solution[i] > T(3.14159265358979) ) { - solution[i] -= T(6.28318530717959); - } - else if( solution[i] < T(-3.14159265358979) ) { - solution[i] += T(6.28318530717959); - } - } + IkSolution(const std::vector >& vinfos, const std::vector& vfree) + { + _vbasesol = vinfos; + _vfree = vfree; + } + + virtual void GetSolution(T* solution, const T* freevalues) const + { + for (std::size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].freeind < 0) + solution[i] = _vbasesol[i].foffset; + else + { + solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset; + if (solution[i] > T(3.14159265358979)) + { + solution[i] -= T(6.28318530717959); } + else if (solution[i] < T(-3.14159265358979)) + { + solution[i] += T(6.28318530717959); + } + } } + } - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + } - virtual const std::vector& GetFree() const { - return _vfree; - } - virtual const int GetDOF() const { - return static_cast(_vbasesol.size()); - } + virtual const std::vector& GetFree() const { return _vfree; } + virtual const int GetDOF() const { return static_cast(_vbasesol.size()); } - virtual void Validate() const { - for(size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].maxsolutions == (unsigned char)-1) { - throw std::runtime_error("max solutions for joint not initialized"); - } - if( _vbasesol[i].maxsolutions > 0 ) { - if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("index >= max solutions for joint"); - } - if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("2nd index >= max solutions for joint"); - } - } + virtual void Validate() const + { + for (size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].maxsolutions == (unsigned char)-1) + { + throw std::runtime_error("max solutions for joint not initialized"); + } + if (_vbasesol[i].maxsolutions > 0) + { + if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("index >= max solutions for joint"); } + if (_vbasesol[i].indices[1] != (unsigned char)-1 && + _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("2nd index >= max solutions for joint"); + } + } } + } - virtual void GetSolutionIndices(std::vector& v) const { - v.resize(0); - v.push_back(0); - for(int i = (int)_vbasesol.size()-1; i >= 0; --i) { - if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) { - for(size_t j = 0; j < v.size(); ++j) { - v[j] *= _vbasesol[i].maxsolutions; - } - size_t orgsize=v.size(); - if( _vbasesol[i].indices[1] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v.push_back(v[j]+_vbasesol[i].indices[1]); - } - } - if( _vbasesol[i].indices[0] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v[j] += _vbasesol[i].indices[0]; - } - } - } + virtual void GetSolutionIndices(std::vector& v) const + { + v.resize(0); + v.push_back(0); + for (int i = (int)_vbasesol.size() - 1; i >= 0; --i) + { + if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1) + { + for (size_t j = 0; j < v.size(); ++j) + { + v[j] *= _vbasesol[i].maxsolutions; } + size_t orgsize = v.size(); + if (_vbasesol[i].indices[1] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v.push_back(v[j] + _vbasesol[i].indices[1]); + } + } + if (_vbasesol[i].indices[0] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v[j] += _vbasesol[i].indices[0]; + } + } + } } + } - std::vector< IkSingleDOFSolutionBase > _vbasesol; ///< solution and their offsets if joints are mimiced - std::vector _vfree; + std::vector > + _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector _vfree; }; /// \brief Default implementation of \ref IkSolutionListBase -template -class IkSolutionList : public IkSolutionListBase +template class IkSolutionList : public IkSolutionListBase { public: - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) - { - size_t index = _listsolutions.size(); - _listsolutions.push_back(IkSolution(vinfos,vfree)); - return index; - } - - virtual const IkSolutionBase& GetSolution(size_t index) const + virtual size_t AddSolution(const std::vector >& vinfos, + const std::vector& vfree) + { + size_t index = _listsolutions.size(); + _listsolutions.push_back(IkSolution(vinfos, vfree)); + return index; + } + + virtual const IkSolutionBase& GetSolution(size_t index) const + { + if (index >= _listsolutions.size()) { - if( index >= _listsolutions.size() ) { - throw std::runtime_error("GetSolution index is invalid"); - } - typename std::list< IkSolution >::const_iterator it = _listsolutions.begin(); - std::advance(it,index); - return *it; + throw std::runtime_error("GetSolution index is invalid"); } + typename std::list >::const_iterator it = _listsolutions.begin(); + std::advance(it, index); + return *it; + } - virtual size_t GetNumSolutions() const { - return _listsolutions.size(); - } + virtual size_t GetNumSolutions() const { return _listsolutions.size(); } - virtual void Clear() { - _listsolutions.clear(); - } + virtual void Clear() { _listsolutions.clear(); } protected: - std::list< IkSolution > _listsolutions; + std::list > _listsolutions; }; - } #endif // OPENRAVE_IKFAST_HEADER @@ -277,7 +304,8 @@ class IkSolutionList : public IkSolutionListBase #endif #ifdef IKFAST_NAMESPACE -namespace IKFAST_NAMESPACE { +namespace IKFAST_NAMESPACE +{ #endif #ifdef IKFAST_REAL @@ -288,16 +316,22 @@ typedef double IkReal; /** \brief Computes all IK solutions given a end effector coordinates and the free joints. - - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. + - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is + the orientation. - ``eerot`` - For **Transform6D** it is 9 values for the 3x3 rotation matrix. - - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. - - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle. - - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. + - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent + the target direction. + - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** + the first value represents the angle. + - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation + inside the end effector coordinate system. */ -IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase& solutions); +IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, + ikfast::IkSolutionListBase& solutions); -/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. +/// \brief Computes the end effector coordinates given the joint values. This function is used to +/// double check ik. IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); /// \brief returns the number of free parameters users has to set apriori diff --git a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/src/plugin_init.cpp b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/src/plugin_init.cpp index 691f5c32..bbdd853e 100644 --- a/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/src/plugin_init.cpp +++ b/godel_robots/abb/godel_irb2400/irb2400_ikfast_manipulator_plugin/src/plugin_init.cpp @@ -1,5 +1,6 @@ -//register IKFastKinematicsPlugin as a KinematicsBase implementation +// register IKFastKinematicsPlugin as a KinematicsBase implementation #include #include -PLUGINLIB_EXPORT_CLASS(irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase); +PLUGINLIB_EXPORT_CLASS(irb2400_ikfast_manipulator_plugin::IKFastKinematicsPlugin, + kinematics::KinematicsBase); diff --git a/godel_scan_analysis/include/godel_scan_analysis/keyence_scan_server.h b/godel_scan_analysis/include/godel_scan_analysis/keyence_scan_server.h index 640b2737..57039ba3 100644 --- a/godel_scan_analysis/include/godel_scan_analysis/keyence_scan_server.h +++ b/godel_scan_analysis/include/godel_scan_analysis/keyence_scan_server.h @@ -13,61 +13,62 @@ namespace godel_scan_analysis { +/** + * @brief Structure for the configuration parameters associated with + * the ScanServer + */ +struct ScanServerConfig +{ + std::string world_frame; + std::string scan_frame; + double voxel_grid_leaf_size; + double voxel_grid_publish_period; +}; + +/** + * Defines the ROS interface for a surface-quality-map + */ +class ScanServer +{ +public: + typedef pcl::PointCloud ColorCloud; + typedef pcl::PointCloud Cloud; + + ScanServer(const ScanServerConfig& config); + /** - * @brief Structure for the configuration parameters associated with - * the ScanServer + * Analyzes the passed-in cloud and adds the scored points to the internal map */ - struct ScanServerConfig - { - std::string world_frame; - std::string scan_frame; - double voxel_grid_leaf_size; - double voxel_grid_publish_period; - }; + void scanCallback(const Cloud& cloud); /** - * Defines the ROS interface for a surface-quality-map + * A debug call-back to publish point-clouds meant for ROS */ - class ScanServer - { - public: - typedef pcl::PointCloud ColorCloud; - typedef pcl::PointCloud Cloud; + void publishCloud(const ros::TimerEvent&) const; - ScanServer(const ScanServerConfig& config); - - /** - * Analyzes the passed-in cloud and adds the scored points to the internal map - */ - void scanCallback(const Cloud& cloud); - - /** - * A debug call-back to publish point-clouds meant for ROS - */ - void publishCloud(const ros::TimerEvent&) const; - - /** - * Queries the underlying map for a colorized point cloud representing the current surface quality of the system - * @return Shared-Pointer to const PointCloud - */ - ColorCloud::ConstPtr getSurfaceQuality() const { return map_; } - - private: - void transformScan(ColorCloud& cloud, const ros::Time& tm) const; - tf::StampedTransform findTransform(const ros::Time& tm) const; + /** + * Queries the underlying map for a colorized point cloud representing the current surface quality + * of the system + * @return Shared-Pointer to const PointCloud + */ + ColorCloud::ConstPtr getSurfaceQuality() const { return map_; } +private: + void transformScan(ColorCloud& cloud, const ros::Time& tm) const; + tf::StampedTransform findTransform(const ros::Time& tm) const; - RoughnessScorer scorer_; /** Object that scores individual lines */ - ColorCloud::Ptr map_; /** Data structure that contains colorised surface quality results */ - ColorCloud::Ptr buffer_; /** Temporarily holds scan results for post-processing and tf lookup */ - tf::TransformListener tf_listener_; // for looking up transforms between laser scan and arm position - ros::Subscriber scan_sub_; // for listening to scans - ros::Publisher cloud_pub_; // for outputting colored clouds of data - ros::Timer timer_; // Publish timer for color cloud - std::string from_frame_; // typically laser_scan_frame - std::string to_frame_; // typically world_frame - ScanServerConfig config_; - }; + RoughnessScorer scorer_; /** Object that scores individual lines */ + ColorCloud::Ptr map_; /** Data structure that contains colorised surface quality results */ + ColorCloud::Ptr buffer_; /** Temporarily holds scan results for post-processing and tf lookup */ + tf::TransformListener + tf_listener_; // for looking up transforms between laser scan and arm position + ros::Subscriber scan_sub_; // for listening to scans + ros::Publisher cloud_pub_; // for outputting colored clouds of data + ros::Timer timer_; // Publish timer for color cloud + std::string from_frame_; // typically laser_scan_frame + std::string to_frame_; // typically world_frame + ScanServerConfig config_; +}; } // end namespace godel_scan_analysis diff --git a/godel_scan_analysis/include/godel_scan_analysis/scan_algorithms.h b/godel_scan_analysis/include/godel_scan_analysis/scan_algorithms.h index f4b44e7a..d7834bef 100644 --- a/godel_scan_analysis/include/godel_scan_analysis/scan_algorithms.h +++ b/godel_scan_analysis/include/godel_scan_analysis/scan_algorithms.h @@ -13,7 +13,7 @@ namespace rms // Utility functions for fitting and using least-squares lines // ///////////////////////////////////////////////////////////////// -template +template LineFitSums calculateSums(InputIt begin, InputIt end) { LineFitSums sums; @@ -36,19 +36,19 @@ LineFitSums calculateSums(InputIt begin, InputIt end) return sums; } -template +template LineCoef calculateLineCoefs(const LineFitSums& sums) -{ +{ FloatType x_mean = sums.x / sums.n; FloatType y_mean = sums.y / sums.n; FloatType slope = (sums.xy - sums.x * y_mean) / (sums.x2 - sums.x * x_mean); FloatType intercept = y_mean - slope * x_mean; - return LineCoef(slope, intercept); + return LineCoef(slope, intercept); } -template +template void adjustWithLineInPlace(const LineCoef& line, IOIter begin, IOIter end) { while (begin != end) @@ -58,13 +58,12 @@ void adjustWithLineInPlace(const LineCoef& line, IOIter begin, IOIter } } - -template +template Scan adjustWithLine(const LineCoef& line, IOIter begin, IOIter end) { Scan scan; scan.points.reserve(std::distance(begin, end)); - + Point temp; while (begin != end) @@ -82,7 +81,7 @@ Scan adjustWithLine(const LineCoef& line, IOIter begin, IO // Utility functions for scoring algorithms // ////////////////////////////////////////////// -template +template inline FloatType sumSquared(InputIt begin, InputIt end, FloatType init = FloatType()) { while (begin != end) @@ -93,7 +92,7 @@ inline FloatType sumSquared(InputIt begin, InputIt end, FloatType init = FloatTy return init; } -template +template inline FloatType sumAbsValue(InputIt begin, InputIt end, FloatType init = FloatType()) { while (begin != end) @@ -108,15 +107,13 @@ inline FloatType sumAbsValue(InputIt begin, InputIt end, FloatType init = FloatT // Surface Roughness Scoring Algorithms // ////////////////////////////////////////// -template -FloatType scoreRms(InputIt begin, InputIt end) +template FloatType scoreRms(InputIt begin, InputIt end) { std::size_t n = std::distance(begin, end); return std::sqrt(sumSquared(begin, end) / n); } -template -FloatType scoreAvgAbs(InputIt begin, InputIt end) +template FloatType scoreAvgAbs(InputIt begin, InputIt end) { std::size_t n = std::distance(begin, end); return sumAbsValue(begin, end) / n; @@ -126,8 +123,8 @@ FloatType scoreAvgAbs(InputIt begin, InputIt end) // Kernel Operations for Applying a Score Function // ///////////////////////////////////////////////////// -template -void kernelOp(InputIt wbegin, InputIt wend, InputIt in_end, OutputIt out_begin, Op op = Op()) +template +void kernelOp(InputIt wbegin, InputIt wend, InputIt in_end, OutputIt out_begin, Op op = Op()) { while (wend != in_end) { diff --git a/godel_scan_analysis/include/godel_scan_analysis/scan_roughness_scoring.h b/godel_scan_analysis/include/godel_scan_analysis/scan_roughness_scoring.h index b0a1e6f2..b77e1523 100644 --- a/godel_scan_analysis/include/godel_scan_analysis/scan_roughness_scoring.h +++ b/godel_scan_analysis/include/godel_scan_analysis/scan_roughness_scoring.h @@ -6,30 +6,31 @@ namespace godel_scan_analysis { - // TODO: add scoring params to this struct - struct ScoringParams {}; - - /** - * This class, once setup, scores individual laser scans - * with a given algorithm, colorizes them, and pushes them - * back out to a colorized cloud. - */ - class RoughnessScorer - { - public: - // Input - typedef pcl::PointCloud Cloud; - // output - typedef pcl::PointCloud ColorCloud; +// TODO: add scoring params to this struct +struct ScoringParams +{ +}; + +/** + * This class, once setup, scores individual laser scans + * with a given algorithm, colorizes them, and pushes them + * back out to a colorized cloud. + */ +class RoughnessScorer +{ +public: + // Input + typedef pcl::PointCloud Cloud; + // output + typedef pcl::PointCloud ColorCloud; - RoughnessScorer(); + RoughnessScorer(); - bool analyze(const Cloud& in, ColorCloud& out) const; + bool analyze(const Cloud& in, ColorCloud& out) const; - private: - ScoringParams params_; - }; +private: + ScoringParams params_; +}; } - #endif diff --git a/godel_scan_analysis/include/godel_scan_analysis/scan_utilities.h b/godel_scan_analysis/include/godel_scan_analysis/scan_utilities.h index 320e634d..ccc9835a 100644 --- a/godel_scan_analysis/include/godel_scan_analysis/scan_utilities.h +++ b/godel_scan_analysis/include/godel_scan_analysis/scan_utilities.h @@ -6,31 +6,22 @@ namespace rms { -template -struct Point +template struct Point { - FloatType x,y; + FloatType x, y; }; -template -struct LineCoef +template struct LineCoef { FloatType slope; FloatType intercept; - LineCoef(double slope, double intercept) - : slope(slope) - , intercept(intercept) - {} + LineCoef(double slope, double intercept) : slope(slope), intercept(intercept) {} - FloatType operator()(const FloatType x) const - { - return slope * x + intercept; - } + FloatType operator()(const FloatType x) const { return slope * x + intercept; } }; -template -struct LineFitSums +template struct LineFitSums { FloatType x; FloatType y; @@ -39,8 +30,7 @@ struct LineFitSums std::size_t n; }; -template -struct Scan +template struct Scan { typedef rms::Point value_type; std::vector points; @@ -49,4 +39,4 @@ struct Scan typedef std::vector Scores; } // end namespace rms -#endif +#endif diff --git a/godel_scan_analysis/src/godel_scan_analysis_node.cpp b/godel_scan_analysis/src/godel_scan_analysis_node.cpp index 0c0389f7..64b2c8bc 100644 --- a/godel_scan_analysis/src/godel_scan_analysis_node.cpp +++ b/godel_scan_analysis/src/godel_scan_analysis_node.cpp @@ -4,13 +4,13 @@ const static std::string DEFAULT_WORLD_FRAME = "world_frame"; const static std::string DEFAULT_SCAN_FRAME = "keyence_sensor_optical_frame"; -const static double VOXEL_GRID_LEAF_SIZE = 0.005; // 5 mm +const static double VOXEL_GRID_LEAF_SIZE = 0.005; // 5 mm const static double VOXEL_GRID_PUBLISH_PERIOD = 2.0; // seconds int main(int argc, char** argv) { ros::init(argc, argv, "scan_server"); - ros::NodeHandle pnh ("~"); + ros::NodeHandle pnh("~"); // Populate a ScanServerConfiguration godel_scan_analysis::ScanServerConfig config; @@ -30,7 +30,8 @@ int main(int argc, char** argv) // Optional params pnh.param("voxel_leaf_size", config.voxel_grid_leaf_size, VOXEL_GRID_LEAF_SIZE); - pnh.param("voxel_publish_period", config.voxel_grid_publish_period, VOXEL_GRID_PUBLISH_PERIOD); + pnh.param("voxel_publish_period", config.voxel_grid_publish_period, + VOXEL_GRID_PUBLISH_PERIOD); godel_scan_analysis::ScanServer server(config); diff --git a/godel_scan_analysis/src/keyence_scan_server.cpp b/godel_scan_analysis/src/keyence_scan_server.cpp index 80dc8c6b..deb8a0e2 100644 --- a/godel_scan_analysis/src/keyence_scan_server.cpp +++ b/godel_scan_analysis/src/keyence_scan_server.cpp @@ -9,9 +9,7 @@ const static double TF_WAIT_TIMEOUT = 0.25; // seconds const static std::string COLOR_CLOUD_TOPIC = "color_cloud"; godel_scan_analysis::ScanServer::ScanServer(const ScanServerConfig& config) - : map_(new ColorCloud) - , buffer_(new ColorCloud) - , config_(config) + : map_(new ColorCloud), buffer_(new ColorCloud), config_(config) { ros::NodeHandle nh; scan_sub_ = nh.subscribe("profiles", 500, &ScanServer::scanCallback, this); @@ -19,13 +17,15 @@ godel_scan_analysis::ScanServer::ScanServer(const ScanServerConfig& config) map_->header.frame_id = config_.world_frame; // Create publisher for the collected color cloud - timer_ = nh.createTimer(ros::Duration(config.voxel_grid_publish_period), &ScanServer::publishCloud, this); + timer_ = nh.createTimer(ros::Duration(config.voxel_grid_publish_period), + &ScanServer::publishCloud, this); } void godel_scan_analysis::ScanServer::scanCallback(const Cloud& cloud) { // Generate colored point cloud of scan data - if (!scorer_.analyze(cloud, *buffer_)) return; + if (!scorer_.analyze(cloud, *buffer_)) + return; // Calculate time stamp was processed ros::Time stamp; @@ -38,7 +38,7 @@ void godel_scan_analysis::ScanServer::scanCallback(const Cloud& cloud) // Insert into results cloud map_->insert(map_->end(), buffer_->begin(), buffer_->end()); } - catch(const tf::TransformException& ex) + catch (const tf::TransformException& ex) { ROS_WARN_STREAM("TF Exception: " << ex.what()); } @@ -53,25 +53,25 @@ void godel_scan_analysis::ScanServer::publishCloud(const ros::TimerEvent&) const // Downsample first pcl::VoxelGrid vg; vg.setInputCloud(map_); - vg.setLeafSize(config_.voxel_grid_leaf_size, config_.voxel_grid_leaf_size, config_.voxel_grid_leaf_size); + vg.setLeafSize(config_.voxel_grid_leaf_size, config_.voxel_grid_leaf_size, + config_.voxel_grid_leaf_size); vg.filter(*pub_cloud); - + cloud_pub_.publish(pub_cloud); } -void godel_scan_analysis::ScanServer::transformScan(ColorCloud& cloud, - const ros::Time& tm) const +void godel_scan_analysis::ScanServer::transformScan(ColorCloud& cloud, const ros::Time& tm) const { tf::StampedTransform transform = findTransform(tm); pcl_ros::transformPointCloud(cloud, cloud, transform); } -inline -tf::StampedTransform godel_scan_analysis::ScanServer::findTransform(const ros::Time& tm) const +inline tf::StampedTransform +godel_scan_analysis::ScanServer::findTransform(const ros::Time& tm) const { tf::StampedTransform transform; - tf_listener_.waitForTransform(config_.world_frame, config_.scan_frame, tm, ros::Duration(TF_WAIT_TIMEOUT)); + tf_listener_.waitForTransform(config_.world_frame, config_.scan_frame, tm, + ros::Duration(TF_WAIT_TIMEOUT)); tf_listener_.lookupTransform(config_.world_frame, config_.scan_frame, tm, transform); return transform; } - diff --git a/godel_scan_analysis/src/scan_roughness_scoring.cpp b/godel_scan_analysis/src/scan_roughness_scoring.cpp index 347bd6f9..590fbff3 100644 --- a/godel_scan_analysis/src/scan_roughness_scoring.cpp +++ b/godel_scan_analysis/src/scan_roughness_scoring.cpp @@ -17,118 +17,117 @@ 6) Pre-allocate or use stack space (std::array?) for storage space */ -const static double DEFAULT_MAX_SCORE = 0.00003; // RMS value about a point that is considered out of spec -const static double DEFAULT_MIN_SCORE = 0.0; // Min RMS value; Used for coloring together with above -const static unsigned WINDOW_SIZE = 30; // The # of points in a window; equivalent to 0.05 mm * WINDOW_SIZE for Keyence +const static double DEFAULT_MAX_SCORE = + 0.00003; // RMS value about a point that is considered out of spec +const static double DEFAULT_MIN_SCORE = 0.0; // Min RMS value; Used for coloring together with above +const static unsigned WINDOW_SIZE = + 30; // The # of points in a window; equivalent to 0.05 mm * WINDOW_SIZE for Keyence namespace { - typedef godel_scan_analysis::RoughnessScorer::Cloud Cloud; - typedef godel_scan_analysis::RoughnessScorer::ColorCloud ColorCloud; +typedef godel_scan_analysis::RoughnessScorer::Cloud Cloud; +typedef godel_scan_analysis::RoughnessScorer::ColorCloud ColorCloud; - - // Preprocess clouds - static rms::Scan filterCloudAndBuildScan(const Cloud& in) +// Preprocess clouds +static rms::Scan filterCloudAndBuildScan(const Cloud& in) +{ + rms::Scan scan; + scan.points.reserve(in.points.size()); + for (std::size_t i = 0; i < in.points.size(); ++i) { - rms::Scan scan; - scan.points.reserve(in.points.size()); - for (std::size_t i = 0; i < in.points.size(); ++i) - { - rms::Point pt; - pt.x = in.points[i].x; - pt.y = in.points[i].z; - - if (std::isfinite(pt.y)) scan.points.push_back(pt); - } - - return scan; - } + rms::Point pt; + pt.x = in.points[i].x; + pt.y = in.points[i].z; - static inline double constrainValue(double min, double max, double val) - { - return (val > max) ? max : ((val < min) ? min : val); + if (std::isfinite(pt.y)) + scan.points.push_back(pt); } - // Takes one point and makes a colored pcl point from it - static pcl::PointXYZRGB makeColoredPoint(const rms::Point& pt, double score) - { - // TODO: put these colorization values into the params struct - static const double max_score = DEFAULT_MAX_SCORE; - static const double min_score = DEFAULT_MIN_SCORE; - - pcl::PointXYZRGB temp; - temp.x = pt.x; - temp.y = 0.0; - temp.z = pt.y; - temp.r = static_cast(constrainValue(min_score, max_score, score) / (max_score-min_score) * 255); - temp.g = 0; - temp.b = 255 - temp.r; - - return temp; - } + return scan; +} - // Generates colored points and inserts into out parameter based on scoring - static void generateColorPoints(const rms::Scan& scan, const rms::Scores& scores, ColorCloud& out) - { - long diff = (scan.points.size() - scores.size()) / 2; - for (size_t i = 0; i < scores.size(); ++i) - { - out.points.push_back( makeColoredPoint(scan.points[i + diff], scores[i]) ); - } - } +static inline double constrainValue(double min, double max, double val) +{ + return (val > max) ? max : ((val < min) ? min : val); +} - typedef std::vector >::iterator scan_iter; +// Takes one point and makes a colored pcl point from it +static pcl::PointXYZRGB makeColoredPoint(const rms::Point& pt, double score) +{ + // TODO: put these colorization values into the params struct + static const double max_score = DEFAULT_MAX_SCORE; + static const double min_score = DEFAULT_MIN_SCORE; + + pcl::PointXYZRGB temp; + temp.x = pt.x; + temp.y = 0.0; + temp.z = pt.y; + temp.r = static_cast(constrainValue(min_score, max_score, score) / + (max_score - min_score) * 255); + temp.g = 0; + temp.b = 255 - temp.r; + + return temp; +} - static double rmsOp(scan_iter a, scan_iter b) +// Generates colored points and inserts into out parameter based on scoring +static void generateColorPoints(const rms::Scan& scan, const rms::Scores& scores, + ColorCloud& out) +{ + long diff = (scan.points.size() - scores.size()) / 2; + for (size_t i = 0; i < scores.size(); ++i) { - return rms::scoreRms(a, b); + out.points.push_back(makeColoredPoint(scan.points[i + diff], scores[i])); } +} - static double absOp(scan_iter a, scan_iter b) - { - return rms::scoreAvgAbs(a, b); - } +typedef std::vector >::iterator scan_iter; - static double localLine(scan_iter a, scan_iter b) - { - // Calculate relevant sums/means - rms::LineFitSums sums = rms::calculateSums(a, b); +static double rmsOp(scan_iter a, scan_iter b) { return rms::scoreRms(a, b); } - // Fit line - rms::LineCoef line = rms::calculateLineCoefs(sums); +static double absOp(scan_iter a, scan_iter b) { return rms::scoreAvgAbs(a, b); } - rms::Scan adjusted = rms::adjustWithLine(line, a, b); +static double localLine(scan_iter a, scan_iter b) +{ + // Calculate relevant sums/means + rms::LineFitSums sums = rms::calculateSums(a, b); - return rms::scoreRms(adjusted.points.begin(), adjusted.points.end()); - } + // Fit line + rms::LineCoef line = rms::calculateLineCoefs(sums); -} // end anon namespace + rms::Scan adjusted = rms::adjustWithLine(line, a, b); + return rms::scoreRms(adjusted.points.begin(), adjusted.points.end()); +} +} // end anon namespace -godel_scan_analysis::RoughnessScorer::RoughnessScorer() -{} +godel_scan_analysis::RoughnessScorer::RoughnessScorer() {} bool godel_scan_analysis::RoughnessScorer::analyze(const Cloud& in, ColorCloud& out) const { // Preprocess rms::Scan scan = filterCloudAndBuildScan(in); - if (scan.points.size() < WINDOW_SIZE) return false; + if (scan.points.size() < WINDOW_SIZE) + return false; // Calculate relevant sums/means - rms::LineFitSums sums = rms::calculateSums(scan.points.begin(), scan.points.end()); + rms::LineFitSums sums = + rms::calculateSums(scan.points.begin(), scan.points.end()); // Fit line rms::LineCoef line = rms::calculateLineCoefs(sums); rms::Scan adjusted = rms::adjustWithLine(line, scan.points.begin(), scan.points.end()); // Reserve space for scores - std::size_t score_size = std::distance(adjusted.points.begin() + WINDOW_SIZE, adjusted.points.end()); - rms::Scores scores (score_size, 0.0); + std::size_t score_size = + std::distance(adjusted.points.begin() + WINDOW_SIZE, adjusted.points.end()); + rms::Scores scores(score_size, 0.0); // Apply a surface roughness scoring function - rms::kernelOp(adjusted.points.begin(), adjusted.points.begin() + WINDOW_SIZE, adjusted.points.end(), scores.begin(), localLine); - + rms::kernelOp(adjusted.points.begin(), adjusted.points.begin() + WINDOW_SIZE, + adjusted.points.end(), scores.begin(), localLine); + // Generate output generateColorPoints(scan, scores, out); diff --git a/godel_simple_gui/include/godel_simple_gui/blending_panel.h b/godel_simple_gui/include/godel_simple_gui/blending_panel.h index 8eced9f7..d1be7e3a 100644 --- a/godel_simple_gui/include/godel_simple_gui/blending_panel.h +++ b/godel_simple_gui/include/godel_simple_gui/blending_panel.h @@ -11,9 +11,8 @@ class BlendingWidget; class BlendingPanel : public rviz::Panel { -Q_OBJECT + Q_OBJECT public: - BlendingPanel(QWidget* parent = 0); virtual ~BlendingPanel(); @@ -22,9 +21,7 @@ Q_OBJECT protected: BlendingWidget* widget_; - }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/blending_widget.h b/godel_simple_gui/include/godel_simple_gui/blending_widget.h index 5831ff13..dea051ef 100644 --- a/godel_simple_gui/include/godel_simple_gui/blending_widget.h +++ b/godel_simple_gui/include/godel_simple_gui/blending_widget.h @@ -9,7 +9,7 @@ namespace Ui { - class BlendingWidget; +class BlendingWidget; } namespace godel_simple_gui @@ -17,13 +17,13 @@ namespace godel_simple_gui class BlendingWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: BlendingWidget(QWidget* parent = 0); virtual ~BlendingWidget(); - // Interface for the states to interact with + // Interface for the states to interact with void setText(const std::string& txt); void setButtonsEnabled(bool enabled); @@ -52,7 +52,7 @@ protected Q_SLOTS: OptionsSubmenu* options_; // Options Subwindow - + // ROS specific stuff ros::NodeHandle nh_; @@ -72,7 +72,6 @@ protected Q_SLOTS: * * And back to the start */ - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/gui_state.h b/godel_simple_gui/include/godel_simple_gui/gui_state.h index 4150c8de..a90444b6 100644 --- a/godel_simple_gui/include/godel_simple_gui/gui_state.h +++ b/godel_simple_gui/include/godel_simple_gui/gui_state.h @@ -9,7 +9,6 @@ namespace godel_simple_gui // Forward declare Main Widget class BlendingWidget; - class GuiState : public QObject { Q_OBJECT @@ -27,9 +26,7 @@ class GuiState : public QObject Q_SIGNALS: void newStateAvailable(GuiState*); - }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/options/blend_tool_configuration.h b/godel_simple_gui/include/godel_simple_gui/options/blend_tool_configuration.h index 39fce9b9..8c3507eb 100644 --- a/godel_simple_gui/include/godel_simple_gui/options/blend_tool_configuration.h +++ b/godel_simple_gui/include/godel_simple_gui/options/blend_tool_configuration.h @@ -5,8 +5,9 @@ #include "godel_msgs/BlendingPlanParameters.h" -namespace Ui { - class BlendToolConfigWindow; +namespace Ui +{ +class BlendToolConfigWindow; } namespace godel_simple_gui @@ -28,7 +29,6 @@ class BlendingPlanConfigWidget : public ParameterWindowBase godel_msgs::BlendingPlanParameters params_; Ui::BlendToolConfigWindow* ui_; }; - } #endif // BLEND_TOOL_CONFIGURATION_H diff --git a/godel_simple_gui/include/godel_simple_gui/options/pose_widget.h b/godel_simple_gui/include/godel_simple_gui/options/pose_widget.h index 9497d5e5..29e7cffc 100644 --- a/godel_simple_gui/include/godel_simple_gui/options/pose_widget.h +++ b/godel_simple_gui/include/godel_simple_gui/options/pose_widget.h @@ -6,27 +6,27 @@ #include #include -namespace Ui { - class PoseWidget; +namespace Ui +{ +class PoseWidget; } namespace godel_simple_gui { -class PoseWidget: public QWidget +class PoseWidget : public QWidget { -Q_OBJECT + Q_OBJECT public: - PoseWidget(QWidget *parent = NULL); + PoseWidget(QWidget* parent = NULL); - void set_values(const geometry_msgs::Pose& p); - void set_values(const tf::Transform &t); - tf::Transform get_values(); + void set_values(const geometry_msgs::Pose& p); + void set_values(const tf::Transform& t); + tf::Transform get_values(); protected: - Ui::PoseWidget* ui_; + Ui::PoseWidget* ui_; }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/options/robot_scan_configuration.h b/godel_simple_gui/include/godel_simple_gui/options/robot_scan_configuration.h index e1c074df..5175a135 100644 --- a/godel_simple_gui/include/godel_simple_gui/options/robot_scan_configuration.h +++ b/godel_simple_gui/include/godel_simple_gui/options/robot_scan_configuration.h @@ -6,8 +6,9 @@ #include "godel_msgs/RobotScanParameters.h" -namespace Ui { - class RobotScanConfigWindow; +namespace Ui +{ +class RobotScanConfigWindow; } namespace godel_simple_gui @@ -28,12 +29,10 @@ class RobotScanConfigWidget : public ParameterWindowBase virtual void update_internal_fields(); protected: - godel_msgs::RobotScanParameters params_; PoseWidget* world_to_obj_pose_widget_; PoseWidget* tcp_to_cam_pose_widget_; Ui::RobotScanConfigWindow* ui_; }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/options/scan_tool_configuration.h b/godel_simple_gui/include/godel_simple_gui/options/scan_tool_configuration.h index 59dd81fa..14af02cc 100644 --- a/godel_simple_gui/include/godel_simple_gui/options/scan_tool_configuration.h +++ b/godel_simple_gui/include/godel_simple_gui/options/scan_tool_configuration.h @@ -5,8 +5,9 @@ #include "godel_msgs/ScanPlanParameters.h" -namespace Ui { - class ScanToolConfigWindow; +namespace Ui +{ +class ScanToolConfigWindow; } namespace godel_simple_gui @@ -21,7 +22,7 @@ class ScanPlanConfigWidget : public ParameterWindowBase virtual void update_display_fields(); virtual void update_internal_fields(); - + static QStringList quality_metric_list; protected: @@ -34,7 +35,6 @@ class ScanPlanConfigWidget : public ParameterWindowBase godel_msgs::ScanPlanParameters params_; Ui::ScanToolConfigWindow* ui_; }; - } #endif // SCAN_TOOL_CONFIGURATION_H diff --git a/godel_simple_gui/include/godel_simple_gui/options/surface_detection_configuration.h b/godel_simple_gui/include/godel_simple_gui/options/surface_detection_configuration.h index dc7d3078..60dbdbc0 100644 --- a/godel_simple_gui/include/godel_simple_gui/options/surface_detection_configuration.h +++ b/godel_simple_gui/include/godel_simple_gui/options/surface_detection_configuration.h @@ -5,8 +5,9 @@ #include #include -namespace Ui { - class SurfaceDetectionConfigWindow; +namespace Ui +{ +class SurfaceDetectionConfigWindow; } namespace godel_simple_gui @@ -29,8 +30,6 @@ class SurfaceDetectionConfigWidget : public ParameterWindowBase godel_msgs::SurfaceDetectionParameters params_; Ui::SurfaceDetectionConfigWindow* ui_; }; - } #endif // SURFACE_DETECTION_CONFIGURATION_H - diff --git a/godel_simple_gui/include/godel_simple_gui/options_submenu.h b/godel_simple_gui/include/godel_simple_gui/options_submenu.h index efe6eff5..1f813b3f 100644 --- a/godel_simple_gui/include/godel_simple_gui/options_submenu.h +++ b/godel_simple_gui/include/godel_simple_gui/options_submenu.h @@ -8,8 +8,9 @@ #include "godel_simple_gui/options/blend_tool_configuration.h" #include "godel_simple_gui/options/scan_tool_configuration.h" -namespace Ui { - class OptionsSubmenu; +namespace Ui +{ +class OptionsSubmenu; } namespace godel_simple_gui @@ -51,4 +52,3 @@ class OptionsSubmenu : public QWidget } // end namespace godel_simple_gui #endif // OPTIONS_SUBMENU_H - diff --git a/godel_simple_gui/include/godel_simple_gui/parameter_window_base.h b/godel_simple_gui/include/godel_simple_gui/parameter_window_base.h index 75117b75..c0a3a423 100644 --- a/godel_simple_gui/include/godel_simple_gui/parameter_window_base.h +++ b/godel_simple_gui/include/godel_simple_gui/parameter_window_base.h @@ -42,8 +42,6 @@ protected Q_SLOTS: virtual void cancel_changes_handler(); virtual void save_changes_handler(); }; - } #endif // PARAMETER_WINDOW_BASE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/error_state.h b/godel_simple_gui/include/godel_simple_gui/states/error_state.h index 8e4975da..321223e2 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/error_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/error_state.h @@ -27,8 +27,6 @@ class ErrorState : public GuiState GuiState* next_state_; QWidget* window_; }; - } #endif // ERROR_STATE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/executing_state.h b/godel_simple_gui/include/godel_simple_gui/states/executing_state.h index 1bab8432..93e8744b 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/executing_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/executing_state.h @@ -31,8 +31,6 @@ class ExecutingState : public GuiState std::vector plan_names_; ros::ServiceClient real_client_; }; - } #endif // EXECUTING_STATE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/planning_state.h b/godel_simple_gui/include/godel_simple_gui/states/planning_state.h index 90cf3293..76f13bd0 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/planning_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/planning_state.h @@ -29,8 +29,6 @@ class PlanningState : public GuiState ros::ServiceClient planning_client_; }; - } #endif // PLANNING_STATE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/scan_teach_state.h b/godel_simple_gui/include/godel_simple_gui/states/scan_teach_state.h index 08898682..f98f9185 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/scan_teach_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/scan_teach_state.h @@ -20,7 +20,6 @@ class ScanTeachState : public GuiState virtual void onBack(BlendingWidget& gui); virtual void onReset(BlendingWidget& gui); }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/states/scanning_state.h b/godel_simple_gui/include/godel_simple_gui/states/scanning_state.h index c71bce62..db7e5f61 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/scanning_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/scanning_state.h @@ -29,7 +29,6 @@ class ScanningState : public GuiState ros::ServiceClient surface_client_; }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/states/simulating_state.h b/godel_simple_gui/include/godel_simple_gui/states/simulating_state.h index 9d481779..ea01c51c 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/simulating_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/simulating_state.h @@ -31,8 +31,6 @@ class SimulatingState : public GuiState std::vector plan_names_; ros::ServiceClient sim_client_; }; - } #endif // SIMULATING_STATE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/surface_select_state.h b/godel_simple_gui/include/godel_simple_gui/states/surface_select_state.h index 06a5b25a..38e7448f 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/surface_select_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/surface_select_state.h @@ -18,7 +18,6 @@ class SurfaceSelectState : public GuiState virtual void onBack(BlendingWidget& gui); virtual void onReset(BlendingWidget& gui); }; - } #endif diff --git a/godel_simple_gui/include/godel_simple_gui/states/wait_for_execute_state.h b/godel_simple_gui/include/godel_simple_gui/states/wait_for_execute_state.h index 9cd89dd1..4124cee6 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/wait_for_execute_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/wait_for_execute_state.h @@ -25,7 +25,5 @@ class WaitToExecuteState : public GuiState private: std::vector plan_names_; }; - } #endif // WAIT_FOR_EXECUTE_STATE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/wait_to_execute_state.h b/godel_simple_gui/include/godel_simple_gui/states/wait_to_execute_state.h index cbbd6790..89c667d6 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/wait_to_execute_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/wait_to_execute_state.h @@ -25,7 +25,5 @@ class WaitToExecuteState : public GuiState private: std::vector plan_names_; }; - } #endif // WAIT_FOR_EXECUTE_STATE_H - diff --git a/godel_simple_gui/include/godel_simple_gui/states/wait_to_simulate_state.h b/godel_simple_gui/include/godel_simple_gui/states/wait_to_simulate_state.h index 975d3d52..25e99d78 100644 --- a/godel_simple_gui/include/godel_simple_gui/states/wait_to_simulate_state.h +++ b/godel_simple_gui/include/godel_simple_gui/states/wait_to_simulate_state.h @@ -23,8 +23,6 @@ class WaitToSimulateState : public GuiState private: std::vector plan_names_; }; - } #endif // WAIT_TO_SIMULATE_STATE_H - diff --git a/godel_simple_gui/src/blending_panel.cpp b/godel_simple_gui/src/blending_panel.cpp index 1d15ed9e..9ce28fcc 100644 --- a/godel_simple_gui/src/blending_panel.cpp +++ b/godel_simple_gui/src/blending_panel.cpp @@ -4,8 +4,7 @@ #include "godel_simple_gui/blending_widget.h" -godel_simple_gui::BlendingPanel::BlendingPanel(QWidget* parent) - : rviz::Panel(parent) +godel_simple_gui::BlendingPanel::BlendingPanel(QWidget* parent) : rviz::Panel(parent) { ROS_INFO("Loaded simple blending panel"); diff --git a/godel_simple_gui/src/blending_widget.cpp b/godel_simple_gui/src/blending_widget.cpp index 249e3bd3..829ede6a 100644 --- a/godel_simple_gui/src/blending_widget.cpp +++ b/godel_simple_gui/src/blending_widget.cpp @@ -9,8 +9,7 @@ #include "godel_simple_gui/states/scan_teach_state.h" godel_simple_gui::BlendingWidget::BlendingWidget(QWidget* parent) - : QWidget(parent) - , active_state_(NULL) + : QWidget(parent), active_state_(NULL) { // UI setup ui_ = new Ui::BlendingWidget; @@ -20,7 +19,7 @@ godel_simple_gui::BlendingWidget::BlendingWidget(QWidget* parent) options_->hide(); // Starts in scan teach state - changeState( new ScanTeachState() ); + changeState(new ScanTeachState()); // Wire in buttons connect(ui_->pushButtonNext, SIGNAL(clicked()), this, SLOT(onNextButton())); @@ -46,34 +45,21 @@ void godel_simple_gui::BlendingWidget::setText(const std::string& txt) ui_->textEditStatus->setPlainText(QString::fromStdString(txt)); } -void godel_simple_gui::BlendingWidget::onNextButton() -{ - active_state_->onNext(*this); -} +void godel_simple_gui::BlendingWidget::onNextButton() { active_state_->onNext(*this); } -void godel_simple_gui::BlendingWidget::onBackButton() -{ - active_state_->onBack(*this); -} +void godel_simple_gui::BlendingWidget::onBackButton() { active_state_->onBack(*this); } -void godel_simple_gui::BlendingWidget::onResetButton() -{ - active_state_->onReset(*this); -} +void godel_simple_gui::BlendingWidget::onResetButton() { active_state_->onReset(*this); } -void godel_simple_gui::BlendingWidget::onOptionsButton() -{ - options_->show(); -} +void godel_simple_gui::BlendingWidget::onOptionsButton() { options_->show(); } -void godel_simple_gui::BlendingWidget::onOptionsSave() -{ -} +void godel_simple_gui::BlendingWidget::onOptionsSave() {} void godel_simple_gui::BlendingWidget::changeState(GuiState* new_state) { // Don't transition to a null new state - if (!new_state) return; + if (!new_state) + return; if (active_state_) { @@ -98,7 +84,9 @@ void godel_simple_gui::BlendingWidget::loadParameters() { godel_msgs::SurfaceBlendingParameters srv; srv.request.action = srv.request.GET_CURRENT_PARAMETERS; - ros::ServiceClient param_client = nodeHandle().serviceClient("surface_blending_parameters"); + ros::ServiceClient param_client = + nodeHandle().serviceClient( + "surface_blending_parameters"); setButtonsEnabled(false); param_client.waitForExistence(); @@ -116,5 +104,3 @@ void godel_simple_gui::BlendingWidget::loadParameters() } setButtonsEnabled(true); } - - diff --git a/godel_simple_gui/src/options/blend_tool_configuration.cpp b/godel_simple_gui/src/options/blend_tool_configuration.cpp index 61c13ef3..c16d325e 100644 --- a/godel_simple_gui/src/options/blend_tool_configuration.cpp +++ b/godel_simple_gui/src/options/blend_tool_configuration.cpp @@ -2,8 +2,9 @@ #include "ui_blend_tool_configuration.h" -godel_simple_gui::BlendingPlanConfigWidget::BlendingPlanConfigWidget(godel_msgs::BlendingPlanParameters params) - : params_(params) +godel_simple_gui::BlendingPlanConfigWidget::BlendingPlanConfigWidget( + godel_msgs::BlendingPlanParameters params) + : params_(params) { ui_ = new Ui::BlendToolConfigWindow(); ui_->setupUi(this); diff --git a/godel_simple_gui/src/options/pose_widget.cpp b/godel_simple_gui/src/options/pose_widget.cpp index 0f249775..520f1c36 100644 --- a/godel_simple_gui/src/options/pose_widget.cpp +++ b/godel_simple_gui/src/options/pose_widget.cpp @@ -10,8 +10,7 @@ #endif // Pose Widget -godel_simple_gui::PoseWidget::PoseWidget(QWidget *parent) - : QWidget(parent) +godel_simple_gui::PoseWidget::PoseWidget(QWidget* parent) : QWidget(parent) { ui_ = new Ui::PoseWidget(); ui_->setupUi(this); @@ -21,15 +20,15 @@ godel_simple_gui::PoseWidget::PoseWidget(QWidget *parent) void godel_simple_gui::PoseWidget::set_values(const geometry_msgs::Pose& p) { tf::Transform t; - tf::poseMsgToTF(p,t); + tf::poseMsgToTF(p, t); set_values(t); } void godel_simple_gui::PoseWidget::set_values(const tf::Transform& t) { tf::Vector3 p = t.getOrigin(); - tfScalar rx,ry,rz; - t.getBasis().getRPY(rx,ry,rz,1); + tfScalar rx, ry, rz; + t.getBasis().getRPY(rx, ry, rz, 1); ui_->LineEditX->setText(QString::number(p.x())); ui_->LineEditY->setText(QString::number(p.y())); ui_->LineEditZ->setText(QString::number(p.z())); @@ -40,7 +39,7 @@ void godel_simple_gui::PoseWidget::set_values(const tf::Transform& t) tf::Transform godel_simple_gui::PoseWidget::get_values() { - double x,y,z,rx,ry,rz; + double x, y, z, rx, ry, rz; x = ui_->LineEditX->text().toDouble(); y = ui_->LineEditY->text().toDouble(); z = ui_->LineEditZ->text().toDouble(); @@ -49,9 +48,9 @@ tf::Transform godel_simple_gui::PoseWidget::get_values() rz = DEG2RAD(ui_->LineEditRz->text().toDouble()); // create transform - tf::Vector3 p(x,y,z); + tf::Vector3 p(x, y, z); tf::Quaternion q; - q.setRPY(rx,ry,rz); + q.setRPY(rx, ry, rz); - return tf::Transform(q,p); + return tf::Transform(q, p); } \ No newline at end of file diff --git a/godel_simple_gui/src/options/robot_scan_configuration.cpp b/godel_simple_gui/src/options/robot_scan_configuration.cpp index ba103f44..587ef31a 100644 --- a/godel_simple_gui/src/options/robot_scan_configuration.cpp +++ b/godel_simple_gui/src/options/robot_scan_configuration.cpp @@ -11,8 +11,9 @@ #endif // Scan Config Widget -godel_simple_gui::RobotScanConfigWidget::RobotScanConfigWidget(godel_msgs::RobotScanParameters params) - : params_(params) +godel_simple_gui::RobotScanConfigWidget::RobotScanConfigWidget( + godel_msgs::RobotScanParameters params) + : params_(params) { ui_ = new Ui::RobotScanConfigWindow(); ui_->setupUi(this); @@ -48,24 +49,21 @@ void godel_simple_gui::RobotScanConfigWidget::update_display_fields() void godel_simple_gui::RobotScanConfigWidget::update_internal_fields() { params_.num_scan_points = ui_->SpinBoxNumScans->value(); - params_.cam_tilt_angle= DEG2RAD(ui_->LineEditCamTilt->text().toDouble()); - params_.cam_to_obj_xoffset= ui_->LineEditCameraXoffset->text().toDouble(); - params_.cam_to_obj_zoffset= ui_->LineEditCameraZoffset->text().toDouble(); - params_.sweep_angle_start= DEG2RAD(ui_->LineEditSweepAngleStart->text().toDouble()); - params_.sweep_angle_end= DEG2RAD(ui_->LineEditSweepAngleEnd->text().toDouble()); + params_.cam_tilt_angle = DEG2RAD(ui_->LineEditCamTilt->text().toDouble()); + params_.cam_to_obj_xoffset = ui_->LineEditCameraXoffset->text().toDouble(); + params_.cam_to_obj_zoffset = ui_->LineEditCameraZoffset->text().toDouble(); + params_.sweep_angle_start = DEG2RAD(ui_->LineEditSweepAngleStart->text().toDouble()); + params_.sweep_angle_end = DEG2RAD(ui_->LineEditSweepAngleEnd->text().toDouble()); params_.reachable_scan_points_ratio = ui_->LineEditReachablePointRatio->text().toDouble(); - params_.scan_topic= ui_->LineEditScanTopic->text().toStdString(); - params_.scan_target_frame= ui_->LineEditScanTargetFrame->text().toStdString(); - params_.world_frame= ui_->LineEditWorldFrame->text().toStdString(); - params_.tcp_frame= ui_->LineEditTcpFrame->text().toStdString(); - params_.group_name= ui_->LineEditGroupName->text().toStdString(); - params_.stop_on_planning_error= ui_->CheckBoxStopOnPlanningError->isChecked(); + params_.scan_topic = ui_->LineEditScanTopic->text().toStdString(); + params_.scan_target_frame = ui_->LineEditScanTargetFrame->text().toStdString(); + params_.world_frame = ui_->LineEditWorldFrame->text().toStdString(); + params_.tcp_frame = ui_->LineEditTcpFrame->text().toStdString(); + params_.group_name = ui_->LineEditGroupName->text().toStdString(); + params_.stop_on_planning_error = ui_->CheckBoxStopOnPlanningError->isChecked(); - tf::poseTFToMsg(world_to_obj_pose_widget_->get_values(),params_.world_to_obj_pose); - tf::poseTFToMsg(tcp_to_cam_pose_widget_->get_values(),params_.tcp_to_cam_pose); + tf::poseTFToMsg(world_to_obj_pose_widget_->get_values(), params_.world_to_obj_pose); + tf::poseTFToMsg(tcp_to_cam_pose_widget_->get_values(), params_.tcp_to_cam_pose); } -godel_simple_gui::RobotScanConfigWidget::~RobotScanConfigWidget() -{ - delete ui_; -} +godel_simple_gui::RobotScanConfigWidget::~RobotScanConfigWidget() { delete ui_; } diff --git a/godel_simple_gui/src/options/scan_tool_configuration.cpp b/godel_simple_gui/src/options/scan_tool_configuration.cpp index 13375a16..9a3bf3ba 100644 --- a/godel_simple_gui/src/options/scan_tool_configuration.cpp +++ b/godel_simple_gui/src/options/scan_tool_configuration.cpp @@ -2,7 +2,7 @@ #include "ui_scan_tool_configuration.h" godel_simple_gui::ScanPlanConfigWidget::ScanPlanConfigWidget(godel_msgs::ScanPlanParameters params) - : params_(params) + : params_(params) { ui_ = new Ui::ScanToolConfigWindow(); ui_->setupUi(this); @@ -66,4 +66,3 @@ int godel_simple_gui::ScanPlanConfigWidget::get_scan_method_enum_value() return -1; } } - diff --git a/godel_simple_gui/src/options/surface_detection_configuration.cpp b/godel_simple_gui/src/options/surface_detection_configuration.cpp index aec2bd02..169e2101 100644 --- a/godel_simple_gui/src/options/surface_detection_configuration.cpp +++ b/godel_simple_gui/src/options/surface_detection_configuration.cpp @@ -1,7 +1,6 @@ #include "godel_simple_gui/options/surface_detection_configuration.h" #include "ui_surface_detection_configuration.h" - #ifndef DEG2RAD #define DEG2RAD(x) ((x)*0.017453293) #endif @@ -10,8 +9,9 @@ #define RAD2DEG(x) ((x)*57.29578) #endif -godel_simple_gui::SurfaceDetectionConfigWidget::SurfaceDetectionConfigWidget(godel_msgs::SurfaceDetectionParameters params) - : params_(params) +godel_simple_gui::SurfaceDetectionConfigWidget::SurfaceDetectionConfigWidget( + godel_msgs::SurfaceDetectionParameters params) + : params_(params) { ui_ = new Ui::SurfaceDetectionConfigWindow(); ui_->setupUi(this); @@ -33,11 +33,13 @@ void godel_simple_gui::SurfaceDetectionConfigWidget::update_display_fields() ui_->LineEditRgMinClusterSize->setText(QString::number(params_.rg_min_cluster_size)); ui_->LineEditRgMaxClusterSize->setText(QString::number(params_.rg_max_cluster_size)); ui_->LineEditRgNeighbors->setText(QString::number(params_.rg_neightbors)); - ui_->LineEditRgSmoothnessThreshold->setText(QString::number(RAD2DEG(params_.rg_smoothness_threshold))); + ui_->LineEditRgSmoothnessThreshold->setText( + QString::number(RAD2DEG(params_.rg_smoothness_threshold))); ui_->LineEditRgCurvatureThreshold->setText(QString::number(params_.rg_curvature_threshold)); ui_->LineEditVoxelLeaf->setText(QString::number(params_.voxel_leafsize)); - ui_->LineEditTabletopSegmentationDist->setText(QString::number(params_.tabletop_seg_distance_threshold)); + ui_->LineEditTabletopSegmentationDist->setText( + QString::number(params_.tabletop_seg_distance_threshold)); ui_->CheckBoxUseTabletopSegmentation->setChecked(static_cast(params_.use_tabletop_seg)); ui_->CheckBoxIgnoreLargestCluster->setChecked(static_cast(params_.ignore_largest_cluster)); @@ -48,9 +50,9 @@ void godel_simple_gui::SurfaceDetectionConfigWidget::update_display_fields() ui_->LineEditTrSearchRadius->setText(QString::number(params_.tr_search_radius)); ui_->LineEditTrMu->setText(QString::number(params_.tr_mu)); ui_->LineEditTrNearestNeighbors->setText(QString::number(params_.tr_max_nearest_neighbors)); - ui_->LineEditTrMaxSurfaceAngle->setText(QString::number(RAD2DEG( params_.tr_max_surface_angle))); - ui_->LineEditTrMinAngle->setText(QString::number(RAD2DEG( params_.tr_min_angle))); - ui_->LineEditTrMaxAngle->setText(QString::number(RAD2DEG( params_.tr_max_angle))); + ui_->LineEditTrMaxSurfaceAngle->setText(QString::number(RAD2DEG(params_.tr_max_surface_angle))); + ui_->LineEditTrMinAngle->setText(QString::number(RAD2DEG(params_.tr_min_angle))); + ui_->LineEditTrMaxAngle->setText(QString::number(RAD2DEG(params_.tr_max_angle))); ui_->CheckBoxTrNormalConsistency->setChecked(static_cast(params_.tr_normal_consistency)); ui_->CheckBoxPaEnabled->setChecked(static_cast(params_.pa_enabled)); @@ -76,7 +78,8 @@ void godel_simple_gui::SurfaceDetectionConfigWidget::update_internal_fields() params_.rg_curvature_threshold = ui_->LineEditRgCurvatureThreshold->text().toDouble(); params_.voxel_leafsize = ui_->LineEditVoxelLeaf->text().toDouble(); - params_.tabletop_seg_distance_threshold = ui_->LineEditTabletopSegmentationDist->text().toDouble(); + params_.tabletop_seg_distance_threshold = + ui_->LineEditTabletopSegmentationDist->text().toDouble(); params_.use_tabletop_seg = ui_->CheckBoxUseTabletopSegmentation->isChecked(); params_.ignore_largest_cluster = ui_->CheckBoxIgnoreLargestCluster->isChecked(); @@ -87,7 +90,7 @@ void godel_simple_gui::SurfaceDetectionConfigWidget::update_internal_fields() params_.tr_search_radius = ui_->LineEditTrSearchRadius->text().toDouble(); params_.tr_mu = ui_->LineEditTrMu->text().toDouble(); params_.tr_max_nearest_neighbors = ui_->LineEditTrNearestNeighbors->text().toDouble(); - params_.tr_max_surface_angle = DEG2RAD( ui_->LineEditTrMaxSurfaceAngle->text().toDouble()); + params_.tr_max_surface_angle = DEG2RAD(ui_->LineEditTrMaxSurfaceAngle->text().toDouble()); params_.tr_min_angle = DEG2RAD(ui_->LineEditTrMinAngle->text().toDouble()); params_.tr_max_angle = DEG2RAD(ui_->LineEditTrMaxAngle->text().toDouble()); params_.tr_normal_consistency = ui_->CheckBoxTrNormalConsistency->isChecked(); diff --git a/godel_simple_gui/src/options_submenu.cpp b/godel_simple_gui/src/options_submenu.cpp index 7d84e914..5c6b2c76 100644 --- a/godel_simple_gui/src/options_submenu.cpp +++ b/godel_simple_gui/src/options_submenu.cpp @@ -4,14 +4,13 @@ #include "godel_simple_gui/options/robot_scan_configuration.h" -godel_simple_gui::OptionsSubmenu::OptionsSubmenu(QWidget *parent) - : QWidget(parent) +godel_simple_gui::OptionsSubmenu::OptionsSubmenu(QWidget* parent) : QWidget(parent) { ui_ = new Ui::OptionsSubmenu(); ui_->setupUi(this); // Set up option menus //// Robot Scan - robot_scan_= new RobotScanConfigWidget(godel_msgs::RobotScanParameters()); + robot_scan_ = new RobotScanConfigWidget(godel_msgs::RobotScanParameters()); connect(ui_->pushButtonScanOptions, SIGNAL(clicked()), robot_scan_, SLOT(show())); //// Surface Detection surface_detection_ = new SurfaceDetectionConfigWidget(godel_msgs::SurfaceDetectionParameters()); @@ -29,40 +28,44 @@ const godel_msgs::RobotScanParameters& godel_simple_gui::OptionsSubmenu::robotSc return robot_scan_->params(); } -void godel_simple_gui::OptionsSubmenu::setRobotScanParams(const godel_msgs::RobotScanParameters& params) +void godel_simple_gui::OptionsSubmenu::setRobotScanParams( + const godel_msgs::RobotScanParameters& params) { robot_scan_->params() = params; robot_scan_->update_display_fields(); } -const godel_msgs::SurfaceDetectionParameters &godel_simple_gui::OptionsSubmenu::surfaceDetectionParams() const +const godel_msgs::SurfaceDetectionParameters& +godel_simple_gui::OptionsSubmenu::surfaceDetectionParams() const { return surface_detection_->params(); } -void godel_simple_gui::OptionsSubmenu::setSurfaceDetectionParams(const godel_msgs::SurfaceDetectionParameters ¶ms) +void godel_simple_gui::OptionsSubmenu::setSurfaceDetectionParams( + const godel_msgs::SurfaceDetectionParameters& params) { surface_detection_->params() = params; surface_detection_->update_display_fields(); } -const godel_msgs::BlendingPlanParameters &godel_simple_gui::OptionsSubmenu::blendingParams() const +const godel_msgs::BlendingPlanParameters& godel_simple_gui::OptionsSubmenu::blendingParams() const { return blend_params_->params(); } -void godel_simple_gui::OptionsSubmenu::setBlendingParams(const godel_msgs::BlendingPlanParameters ¶ms) +void godel_simple_gui::OptionsSubmenu::setBlendingParams( + const godel_msgs::BlendingPlanParameters& params) { blend_params_->params() = params; blend_params_->update_display_fields(); } -const godel_msgs::ScanPlanParameters &godel_simple_gui::OptionsSubmenu::scanParams() const +const godel_msgs::ScanPlanParameters& godel_simple_gui::OptionsSubmenu::scanParams() const { return scan_params_->params(); } -void godel_simple_gui::OptionsSubmenu::setScanParams(const godel_msgs::ScanPlanParameters ¶ms) +void godel_simple_gui::OptionsSubmenu::setScanParams(const godel_msgs::ScanPlanParameters& params) { scan_params_->params() = params; scan_params_->update_display_fields(); diff --git a/godel_simple_gui/src/parameter_window_base.cpp b/godel_simple_gui/src/parameter_window_base.cpp index 32bb4757..0c738a80 100644 --- a/godel_simple_gui/src/parameter_window_base.cpp +++ b/godel_simple_gui/src/parameter_window_base.cpp @@ -14,14 +14,10 @@ void godel_simple_gui::ParameterWindowBase::accept_changes_handler() hide(); } -void godel_simple_gui::ParameterWindowBase::cancel_changes_handler() -{ - hide(); -} +void godel_simple_gui::ParameterWindowBase::cancel_changes_handler() { hide(); } void godel_simple_gui::ParameterWindowBase::save_changes_handler() { accept_changes_handler(); Q_EMIT parameters_save_requested(); } - diff --git a/godel_simple_gui/src/states/error_state.cpp b/godel_simple_gui/src/states/error_state.cpp index f09c4024..a3ec5d57 100644 --- a/godel_simple_gui/src/states/error_state.cpp +++ b/godel_simple_gui/src/states/error_state.cpp @@ -6,9 +6,9 @@ #include godel_simple_gui::ErrorState::ErrorState(const std::string& msg, GuiState* next_state) - : msg_(msg) - , next_state_(next_state) -{} + : msg_(msg), next_state_(next_state) +{ +} void godel_simple_gui::ErrorState::onStart(BlendingWidget& gui) { @@ -19,7 +19,8 @@ void godel_simple_gui::ErrorState::onStart(BlendingWidget& gui) window_ = new QWidget(); QMessageBox::StandardButton reply; - reply = QMessageBox::critical(window_, tr("Warning"), QString::fromStdString(msg_), QMessageBox::Ok); + reply = + QMessageBox::critical(window_, tr("Warning"), QString::fromStdString(msg_), QMessageBox::Ok); Q_EMIT newStateAvailable(next_state_); } @@ -30,14 +31,8 @@ void godel_simple_gui::ErrorState::onExit(BlendingWidget& gui) } // Handlers for the fixed buttons -void godel_simple_gui::ErrorState::onNext(BlendingWidget& gui) -{ -} +void godel_simple_gui::ErrorState::onNext(BlendingWidget& gui) {} -void godel_simple_gui::ErrorState::onBack(BlendingWidget& gui) -{ -} +void godel_simple_gui::ErrorState::onBack(BlendingWidget& gui) {} -void godel_simple_gui::ErrorState::onReset(BlendingWidget& gui) -{ -} +void godel_simple_gui::ErrorState::onReset(BlendingWidget& gui) {} diff --git a/godel_simple_gui/src/states/executing_state.cpp b/godel_simple_gui/src/states/executing_state.cpp index 8182edd4..b07e7565 100644 --- a/godel_simple_gui/src/states/executing_state.cpp +++ b/godel_simple_gui/src/states/executing_state.cpp @@ -14,43 +14,35 @@ const static std::string SELECT_MOTION_PLAN_SERVICE = "select_motion_plan"; -struct BadExecutionError { - BadExecutionError(const std::string& what) - : what(what) - {} +struct BadExecutionError +{ + BadExecutionError(const std::string& what) : what(what) {} std::string what; }; -godel_simple_gui::ExecutingState::ExecutingState(const std::vector &plans) - : plan_names_(plans) -{} +godel_simple_gui::ExecutingState::ExecutingState(const std::vector& plans) + : plan_names_(plans) +{ +} void godel_simple_gui::ExecutingState::onStart(BlendingWidget& gui) { gui.setText("Executing..."); gui.setButtonsEnabled(false); - real_client_ = gui.nodeHandle().serviceClient(SELECT_MOTION_PLAN_SERVICE); + real_client_ = + gui.nodeHandle().serviceClient(SELECT_MOTION_PLAN_SERVICE); QtConcurrent::run(this, &ExecutingState::executeAll); } -void godel_simple_gui::ExecutingState::onExit(BlendingWidget& gui) -{ - gui.setButtonsEnabled(true); -} +void godel_simple_gui::ExecutingState::onExit(BlendingWidget& gui) { gui.setButtonsEnabled(true); } // Handlers for the fixed buttons -void godel_simple_gui::ExecutingState::onNext(BlendingWidget& gui) -{ -} +void godel_simple_gui::ExecutingState::onNext(BlendingWidget& gui) {} -void godel_simple_gui::ExecutingState::onBack(BlendingWidget& gui) -{ -} +void godel_simple_gui::ExecutingState::onBack(BlendingWidget& gui) {} -void godel_simple_gui::ExecutingState::onReset(BlendingWidget& gui) -{ -} +void godel_simple_gui::ExecutingState::onReset(BlendingWidget& gui) {} void godel_simple_gui::ExecutingState::executeAll() { @@ -60,17 +52,17 @@ void godel_simple_gui::ExecutingState::executeAll() { executeOne(plan_names_[i]); } - - Q_EMIT newStateAvailable( new WaitToExecuteState(plan_names_) ); + + Q_EMIT newStateAvailable(new WaitToExecuteState(plan_names_)); } catch (const BadExecutionError& err) { ROS_ERROR_STREAM("There was an error executing a plan " << err.what); - Q_EMIT newStateAvailable( new ErrorState(err.what, new WaitToExecuteState(plan_names_)) ); + Q_EMIT newStateAvailable(new ErrorState(err.what, new WaitToExecuteState(plan_names_))); } } -void godel_simple_gui::ExecutingState::executeOne(const std::string &plan) +void godel_simple_gui::ExecutingState::executeOne(const std::string& plan) { ROS_DEBUG_STREAM("Executing " << plan); godel_msgs::SelectMotionPlan srv; @@ -84,8 +76,3 @@ void godel_simple_gui::ExecutingState::executeOne(const std::string &plan) throw BadExecutionError(ss.str()); } } - - - - - diff --git a/godel_simple_gui/src/states/planning_state.cpp b/godel_simple_gui/src/states/planning_state.cpp index 86b68112..bb2286c7 100644 --- a/godel_simple_gui/src/states/planning_state.cpp +++ b/godel_simple_gui/src/states/planning_state.cpp @@ -18,28 +18,20 @@ void godel_simple_gui::PlanningState::onStart(BlendingWidget& gui) gui.setText("Planning..."); gui.setButtonsEnabled(false); - planning_client_ = gui.nodeHandle().serviceClient(PROCESS_PATH_SERVICE); - QtConcurrent::run(this, &PlanningState::makeRequest, - gui.options().blendingParams(), gui.options().scanParams()); + planning_client_ = + gui.nodeHandle().serviceClient(PROCESS_PATH_SERVICE); + QtConcurrent::run(this, &PlanningState::makeRequest, gui.options().blendingParams(), + gui.options().scanParams()); } -void godel_simple_gui::PlanningState::onExit(BlendingWidget& gui) -{ - gui.setButtonsEnabled(true); -} +void godel_simple_gui::PlanningState::onExit(BlendingWidget& gui) { gui.setButtonsEnabled(true); } // Handlers for the fixed buttons -void godel_simple_gui::PlanningState::onNext(BlendingWidget& gui) -{ -} +void godel_simple_gui::PlanningState::onNext(BlendingWidget& gui) {} -void godel_simple_gui::PlanningState::onBack(BlendingWidget& gui) -{ -} +void godel_simple_gui::PlanningState::onBack(BlendingWidget& gui) {} -void godel_simple_gui::PlanningState::onReset(BlendingWidget& gui) -{ -} +void godel_simple_gui::PlanningState::onReset(BlendingWidget& gui) {} void godel_simple_gui::PlanningState::makeRequest(godel_msgs::BlendingPlanParameters blend_params, godel_msgs::ScanPlanParameters scan_params) @@ -52,13 +44,11 @@ void godel_simple_gui::PlanningState::makeRequest(godel_msgs::BlendingPlanParame if (planning_client_.call(srv)) { - Q_EMIT newStateAvailable( new WaitToSimulateState() ); + Q_EMIT newStateAvailable(new WaitToSimulateState()); } else { ROS_WARN_STREAM("Process planning failed"); - Q_EMIT newStateAvailable( new SurfaceSelectState() ); + Q_EMIT newStateAvailable(new SurfaceSelectState()); } } - - diff --git a/godel_simple_gui/src/states/scan_teach_state.cpp b/godel_simple_gui/src/states/scan_teach_state.cpp index cfd08ef8..fbf03c37 100644 --- a/godel_simple_gui/src/states/scan_teach_state.cpp +++ b/godel_simple_gui/src/states/scan_teach_state.cpp @@ -4,27 +4,19 @@ #include #include "godel_simple_gui/blending_widget.h" - void godel_simple_gui::ScanTeachState::onStart(BlendingWidget& gui) { gui.setText("Ready to Scan\nPress Next to Continue"); } -void godel_simple_gui::ScanTeachState::onExit(BlendingWidget& gui) -{ -} +void godel_simple_gui::ScanTeachState::onExit(BlendingWidget& gui) {} // Handlers for the fixed buttons void godel_simple_gui::ScanTeachState::onNext(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new ScanningState() ); -} - -void godel_simple_gui::ScanTeachState::onBack(BlendingWidget& gui) -{ + Q_EMIT newStateAvailable(new ScanningState()); } -void godel_simple_gui::ScanTeachState::onReset(BlendingWidget& gui) -{ -} +void godel_simple_gui::ScanTeachState::onBack(BlendingWidget& gui) {} +void godel_simple_gui::ScanTeachState::onReset(BlendingWidget& gui) {} diff --git a/godel_simple_gui/src/states/scanning_state.cpp b/godel_simple_gui/src/states/scanning_state.cpp index ba6aea58..5b404b7b 100644 --- a/godel_simple_gui/src/states/scanning_state.cpp +++ b/godel_simple_gui/src/states/scanning_state.cpp @@ -19,32 +19,24 @@ void godel_simple_gui::ScanningState::onStart(BlendingWidget& gui) { gui.setButtonsEnabled(false); gui.setText("Scanning..."); - surface_client_ = gui.nodeHandle().serviceClient(SURFACE_DETECTION_SERVICE); - QtConcurrent::run(this, &ScanningState::makeRequest, - gui.options().robotScanParams(), - gui.options().surfaceDetectionParams()); + surface_client_ = + gui.nodeHandle().serviceClient(SURFACE_DETECTION_SERVICE); + QtConcurrent::run(this, &ScanningState::makeRequest, gui.options().robotScanParams(), + gui.options().surfaceDetectionParams()); } -void godel_simple_gui::ScanningState::onExit(BlendingWidget& gui) -{ - gui.setButtonsEnabled(true); -} +void godel_simple_gui::ScanningState::onExit(BlendingWidget& gui) { gui.setButtonsEnabled(true); } // Handlers for the fixed buttons -void godel_simple_gui::ScanningState::onNext(BlendingWidget& gui) -{ -} +void godel_simple_gui::ScanningState::onNext(BlendingWidget& gui) {} -void godel_simple_gui::ScanningState::onBack(BlendingWidget& gui) -{ -} +void godel_simple_gui::ScanningState::onBack(BlendingWidget& gui) {} -void godel_simple_gui::ScanningState::onReset(BlendingWidget& gui) -{ -} +void godel_simple_gui::ScanningState::onReset(BlendingWidget& gui) {} -void godel_simple_gui::ScanningState::makeRequest(godel_msgs::RobotScanParameters scan_params, - godel_msgs::SurfaceDetectionParameters surface_params) +void godel_simple_gui::ScanningState::makeRequest( + godel_msgs::RobotScanParameters scan_params, + godel_msgs::SurfaceDetectionParameters surface_params) { godel_msgs::SurfaceDetection srv; srv.request.action = srv.request.SCAN_AND_FIND_ONLY; @@ -54,11 +46,10 @@ void godel_simple_gui::ScanningState::makeRequest(godel_msgs::RobotScanParameter if (!surface_client_.call(srv)) { ROS_WARN_STREAM("Unable to call surface detection service"); - Q_EMIT newStateAvailable( new ScanTeachState() ); + Q_EMIT newStateAvailable(new ScanTeachState()); } else { - Q_EMIT newStateAvailable( new SurfaceSelectState() ); + Q_EMIT newStateAvailable(new SurfaceSelectState()); } } - diff --git a/godel_simple_gui/src/states/simulating_state.cpp b/godel_simple_gui/src/states/simulating_state.cpp index 4676d514..eabe95d4 100644 --- a/godel_simple_gui/src/states/simulating_state.cpp +++ b/godel_simple_gui/src/states/simulating_state.cpp @@ -1,7 +1,7 @@ #include "godel_simple_gui/states/simulating_state.h" // prev #include "godel_simple_gui/states/wait_to_simulate_state.h" - //next +// next #include "godel_simple_gui/states/wait_to_execute_state.h" #include @@ -13,36 +13,29 @@ const static std::string SELECT_MOTION_PLAN_SERVICE = "select_motion_plan"; -godel_simple_gui::SimulatingState::SimulatingState(const std::vector &plans) - : plan_names_(plans) -{} +godel_simple_gui::SimulatingState::SimulatingState(const std::vector& plans) + : plan_names_(plans) +{ +} void godel_simple_gui::SimulatingState::onStart(BlendingWidget& gui) { gui.setText("Simulating..."); gui.setButtonsEnabled(false); - sim_client_ = gui.nodeHandle().serviceClient(SELECT_MOTION_PLAN_SERVICE); + sim_client_ = + gui.nodeHandle().serviceClient(SELECT_MOTION_PLAN_SERVICE); QtConcurrent::run(this, &SimulatingState::simulateAll); } -void godel_simple_gui::SimulatingState::onExit(BlendingWidget& gui) -{ - gui.setButtonsEnabled(true); -} +void godel_simple_gui::SimulatingState::onExit(BlendingWidget& gui) { gui.setButtonsEnabled(true); } // Handlers for the fixed buttons -void godel_simple_gui::SimulatingState::onNext(BlendingWidget& gui) -{ -} +void godel_simple_gui::SimulatingState::onNext(BlendingWidget& gui) {} -void godel_simple_gui::SimulatingState::onBack(BlendingWidget& gui) -{ -} +void godel_simple_gui::SimulatingState::onBack(BlendingWidget& gui) {} -void godel_simple_gui::SimulatingState::onReset(BlendingWidget& gui) -{ -} +void godel_simple_gui::SimulatingState::onReset(BlendingWidget& gui) {} void godel_simple_gui::SimulatingState::simulateAll() { @@ -51,10 +44,10 @@ void godel_simple_gui::SimulatingState::simulateAll() simulateOne(plan_names_[i]); } - Q_EMIT newStateAvailable( new WaitToExecuteState(plan_names_) ); + Q_EMIT newStateAvailable(new WaitToExecuteState(plan_names_)); } -void godel_simple_gui::SimulatingState::simulateOne(const std::string &plan) +void godel_simple_gui::SimulatingState::simulateOne(const std::string& plan) { godel_msgs::SelectMotionPlan srv; srv.request.name = plan; @@ -65,8 +58,3 @@ void godel_simple_gui::SimulatingState::simulateOne(const std::string &plan) ROS_WARN_STREAM("Client simulation request returned false"); } } - - - - - diff --git a/godel_simple_gui/src/states/surface_select_state.cpp b/godel_simple_gui/src/states/surface_select_state.cpp index 4402952d..b9bf715c 100644 --- a/godel_simple_gui/src/states/surface_select_state.cpp +++ b/godel_simple_gui/src/states/surface_select_state.cpp @@ -10,23 +10,20 @@ void godel_simple_gui::SurfaceSelectState::onStart(BlendingWidget& gui) gui.setText("Pick Surfaces With Mouse\nPress Next to Continue"); } -void godel_simple_gui::SurfaceSelectState::onExit(BlendingWidget& gui) -{ -} +void godel_simple_gui::SurfaceSelectState::onExit(BlendingWidget& gui) {} // Handlers for the fixed buttons void godel_simple_gui::SurfaceSelectState::onNext(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new PlanningState() ); + Q_EMIT newStateAvailable(new PlanningState()); } void godel_simple_gui::SurfaceSelectState::onBack(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new ScanTeachState() ); + Q_EMIT newStateAvailable(new ScanTeachState()); } void godel_simple_gui::SurfaceSelectState::onReset(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new ScanTeachState() ); + Q_EMIT newStateAvailable(new ScanTeachState()); } - diff --git a/godel_simple_gui/src/states/wait_to_execute_state.cpp b/godel_simple_gui/src/states/wait_to_execute_state.cpp index eadb289c..58c0d916 100644 --- a/godel_simple_gui/src/states/wait_to_execute_state.cpp +++ b/godel_simple_gui/src/states/wait_to_execute_state.cpp @@ -9,9 +9,10 @@ #include #include "godel_simple_gui/blending_widget.h" -godel_simple_gui::WaitToExecuteState::WaitToExecuteState(const std::vector &plans) - : plan_names_(plans) -{} +godel_simple_gui::WaitToExecuteState::WaitToExecuteState(const std::vector& plans) + : plan_names_(plans) +{ +} void godel_simple_gui::WaitToExecuteState::onStart(BlendingWidget& gui) { @@ -28,17 +29,17 @@ void godel_simple_gui::WaitToExecuteState::onExit(BlendingWidget& gui) void godel_simple_gui::WaitToExecuteState::onNext(BlendingWidget& gui) { ROS_INFO_STREAM("WaitToExecuteState next"); - Q_EMIT newStateAvailable( new ExecutingState(plan_names_) ); + Q_EMIT newStateAvailable(new ExecutingState(plan_names_)); } void godel_simple_gui::WaitToExecuteState::onBack(BlendingWidget& gui) { ROS_INFO_STREAM("WaitToExecuteState back"); - Q_EMIT newStateAvailable( new WaitToSimulateState() ); + Q_EMIT newStateAvailable(new WaitToSimulateState()); } void godel_simple_gui::WaitToExecuteState::onReset(BlendingWidget& gui) { ROS_INFO_STREAM("WaitToExecuteState reset"); - Q_EMIT newStateAvailable( new ScanTeachState() ); + Q_EMIT newStateAvailable(new ScanTeachState()); } diff --git a/godel_simple_gui/src/states/wait_to_simulate_state.cpp b/godel_simple_gui/src/states/wait_to_simulate_state.cpp index 8e61c9fc..ccdac0b6 100644 --- a/godel_simple_gui/src/states/wait_to_simulate_state.cpp +++ b/godel_simple_gui/src/states/wait_to_simulate_state.cpp @@ -21,8 +21,8 @@ void godel_simple_gui::WaitToSimulateState::onStart(BlendingWidget& gui) { gui.setText("Ready to Simulate.\nPress 'Next' to observe the plan."); - ros::ServiceClient client = - gui.nodeHandle().serviceClient(GET_AVAILABLE_MOTION_PLANS_SERVICE); + ros::ServiceClient client = gui.nodeHandle().serviceClient( + GET_AVAILABLE_MOTION_PLANS_SERVICE); godel_msgs::GetAvailableMotionPlans srv; if (client.call(srv)) @@ -36,31 +36,26 @@ void godel_simple_gui::WaitToSimulateState::onStart(BlendingWidget& gui) if (plan_names_.empty()) { - Q_EMIT newStateAvailable( new ErrorState("No motion plans available. " - "Please check surface selections and try again", - new SurfaceSelectState()) ); + Q_EMIT newStateAvailable(new ErrorState("No motion plans available. " + "Please check surface selections and try again", + new SurfaceSelectState())); } } -void godel_simple_gui::WaitToSimulateState::onExit(BlendingWidget& gui) -{ -} +void godel_simple_gui::WaitToSimulateState::onExit(BlendingWidget& gui) {} // Handlers for the fixed buttons void godel_simple_gui::WaitToSimulateState::onNext(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new SimulatingState(plan_names_) ); + Q_EMIT newStateAvailable(new SimulatingState(plan_names_)); } void godel_simple_gui::WaitToSimulateState::onBack(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new SurfaceSelectState() ); + Q_EMIT newStateAvailable(new SurfaceSelectState()); } void godel_simple_gui::WaitToSimulateState::onReset(BlendingWidget& gui) { - Q_EMIT newStateAvailable( new ScanTeachState() ); + Q_EMIT newStateAvailable(new ScanTeachState()); } - - - diff --git a/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h b/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h index e82ad5b0..c8794b97 100644 --- a/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h +++ b/godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h @@ -1,17 +1,17 @@ /* - Copyright Feb 11, 2014 Southwest Research Institute + Copyright Feb 11, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #ifndef SURFACE_DETECTION_H_ @@ -28,193 +28,187 @@ #include #include -namespace godel_surface_detection { namespace detection{ +namespace godel_surface_detection +{ +namespace detection +{ typedef pcl::PointCloud Cloud; typedef pcl::PointCloud CloudRGB; typedef pcl::PointCloud Normals; -namespace defaults{ - - //static const double ACQUISITION_TIME = 5.0f; - static std::string FRAME_ID = "world_frame"; +namespace defaults +{ - static const int STATISTICAL_OUTLIER_MEAN = 50; - static const double STATISTICAL_OUTLIER_STDEV_THRESHOLD = 1; - static const int K_SEARCH = 50; +// static const double ACQUISITION_TIME = 5.0f; +static std::string FRAME_ID = "world_frame"; - static const int REGION_GROWING_MIN_CLUSTER_SIZE=100; - static const int REGION_GROWING_MAX_CLUSTER_SIZE=100000; - static const int REGION_GROWING_NEIGHBORS=50; - static const double REGION_GROWING_SMOOTHNESS_THRESHOLD=(M_PI/180.0f) * 7.0f; - static const double REGION_GROWING_CURVATURE_THRESHOLD=1.0f; +static const int STATISTICAL_OUTLIER_MEAN = 50; +static const double STATISTICAL_OUTLIER_STDEV_THRESHOLD = 1; +static const int K_SEARCH = 50; - static const double TRIANGULATION_SEARCH_RADIUS = 0.01f; - static const double TRIANGULATION_MU = 2.5f; - static const int TRIANGULATION_MAX_NEAREST_NEIGHBORS = 100; - static const double TRIANGULATION_MAX_SURFACE_ANGLE = M_PI/4.0f; - static const double TRIANGULATION_MIN_ANGLE= M_PI/18.0f; - static const double TRIANGULATION_MAX_ANGLE = 2.0f *M_PI/3.0f; - static const bool TRIANGULATION_NORMAL_CONSISTENCY = false; +static const int REGION_GROWING_MIN_CLUSTER_SIZE = 100; +static const int REGION_GROWING_MAX_CLUSTER_SIZE = 100000; +static const int REGION_GROWING_NEIGHBORS = 50; +static const double REGION_GROWING_SMOOTHNESS_THRESHOLD = (M_PI / 180.0f) * 7.0f; +static const double REGION_GROWING_CURVATURE_THRESHOLD = 1.0f; - static const bool PLANE_APROX_REFINEMENT_ENABLED = true; - static const int PLANE_APROX_REFINEMENT_SEG_MAX_ITERATIONS = 100; - static const double PLANE_APROX_REFINEMENT_SEG_DIST_THRESHOLD = 0.01f; - static const double PLANE_APROX_REFINEMENT_SAC_PLANE_DISTANCE = 0.01f; - static const std::string PLANE_APROX_REFINEMENT_KDTREE_RADIUS = "pa_kdtree_radius"; +static const double TRIANGULATION_SEARCH_RADIUS = 0.01f; +static const double TRIANGULATION_MU = 2.5f; +static const int TRIANGULATION_MAX_NEAREST_NEIGHBORS = 100; +static const double TRIANGULATION_MAX_SURFACE_ANGLE = M_PI / 4.0f; +static const double TRIANGULATION_MIN_ANGLE = M_PI / 18.0f; +static const double TRIANGULATION_MAX_ANGLE = 2.0f * M_PI / 3.0f; +static const bool TRIANGULATION_NORMAL_CONSISTENCY = false; +static const bool PLANE_APROX_REFINEMENT_ENABLED = true; +static const int PLANE_APROX_REFINEMENT_SEG_MAX_ITERATIONS = 100; +static const double PLANE_APROX_REFINEMENT_SEG_DIST_THRESHOLD = 0.01f; +static const double PLANE_APROX_REFINEMENT_SAC_PLANE_DISTANCE = 0.01f; +static const std::string PLANE_APROX_REFINEMENT_KDTREE_RADIUS = "pa_kdtree_radius"; - static const double VOXEL_LEAF_SIZE = 0.01f; +static const double VOXEL_LEAF_SIZE = 0.01f; - static const double OCCUPANCY_THRESHOLD = 0.1f; +static const double OCCUPANCY_THRESHOLD = 0.1f; - // Moving least square smoothing - static const double MLS_UPSAMPLING_RADIUS = 0.01f; - static const double MLS_SEARCH_RADIUS = 0.01f; - static const int MLS_POINT_DENSITY = 40; +// Moving least square smoothing +static const double MLS_UPSAMPLING_RADIUS = 0.01f; +static const double MLS_SEARCH_RADIUS = 0.01f; +static const int MLS_POINT_DENSITY = 40; - static const bool USE_TABLETOP_SEGMENTATION = true; - static const double TABLETOP_SEG_DISTANCE_THRESH = 0.005f; +static const bool USE_TABLETOP_SEGMENTATION = true; +static const double TABLETOP_SEG_DISTANCE_THRESH = 0.005f; - static const double MARKER_ALPHA = 1.0f; - static const bool IGNORE_LARGEST_CLUSTER = false; +static const double MARKER_ALPHA = 1.0f; +static const bool IGNORE_LARGEST_CLUSTER = false; } namespace config { - static const std::string POINT_COLUD_TOPIC="sensor_point_cloud"; +static const std::string POINT_COLUD_TOPIC = "sensor_point_cloud"; } namespace params { - static const std::string FRAME_ID = "frame_id"; - static const std::string USE_OCTOMAP = "use_octomap"; - - static const std::string STOUTLIER_MEAN = "stout_mean"; - static const std::string STOUTLIER_STDEV_THRESHOLD = "stout_stdev_threshold"; - static const std::string K_SEARCH = "k_search"; - - static const std::string REGION_GROWING_MIN_CLUSTER_SIZE="rg_min_cluster_size"; - static const std::string REGION_GROWING_MAX_CLUSTER_SIZE="rg_max_cluster_size"; - static const std::string REGION_GROWING_NEIGHBORS="rg_neighbors"; - static const std::string REGION_GROWING_SMOOTHNESS_THRESHOLD="rg_smoothness_threshold"; - static const std::string REGION_GROWING_CURVATURE_THRESHOLD="rg_curvature_threshold"; - - static const std::string TRIANGULATION_SEARCH_RADIUS = "tr_search_radius"; - static const std::string TRIANGULATION_MU = "tr_mu"; - static const std::string TRIANGULATION_MAX_NEAREST_NEIGHBORS = "tr_nearest_neighbors"; - static const std::string TRIANGULATION_MAX_SURFACE_ANGLE ="tr_max_surface_angle"; - static const std::string TRIANGULATION_MIN_ANGLE= "tr_min_angle"; - static const std::string TRIANGULATION_MAX_ANGLE = "tr_max_angle"; - static const std::string TRIANGULATION_NORMAL_CONSISTENCY = "tr_normal_consistency"; - - static const std::string PLANE_APROX_REFINEMENT_ENABLED = "pa_enabled"; - static const std::string PLANE_APROX_REFINEMENT_SEG_MAX_ITERATIONS = "pa_seg_max_iterations"; - static const std::string PLANE_APROX_REFINEMENT_SEG_DIST_THRESHOLD = "pa_seg_dist_threshold"; - static const std::string PLANE_APROX_REFINEMENT_SAC_PLANE_DISTANCE = "pa_sac_plane_distance"; - static const std::string PLANE_APROX_REFINEMENT_KDTREE_RADIUS = "pa_kdtree_radius"; - - static const std::string VOXEL_LEAF_SIZE = "voxel_leaf"; - - static const std::string OCCUPANCY_THRESHOLD = "occupancy_threshold"; - - static const std::string MLS_UPSAMPLING_RADIUS = "mls_upsampling_radius"; - static const std::string MLS_SEARCH_RADIUS = "mls_search_radius"; - static const std::string MLS_POINT_DENSITY = "mls_point_density"; - - static const std::string USE_TABLETOP_SEGMENTATION = "use_tabletop_segmentation"; - static const std::string TABLETOP_SEG_DISTANCE_THRESH = "tabletop_seg_distance_thresh"; - - static const std::string MARKER_ALPHA = "marker_alpha"; - static const std::string IGNORE_LARGEST_CLUSTER = "ignore_largest_cluster"; - +static const std::string FRAME_ID = "frame_id"; +static const std::string USE_OCTOMAP = "use_octomap"; + +static const std::string STOUTLIER_MEAN = "stout_mean"; +static const std::string STOUTLIER_STDEV_THRESHOLD = "stout_stdev_threshold"; +static const std::string K_SEARCH = "k_search"; + +static const std::string REGION_GROWING_MIN_CLUSTER_SIZE = "rg_min_cluster_size"; +static const std::string REGION_GROWING_MAX_CLUSTER_SIZE = "rg_max_cluster_size"; +static const std::string REGION_GROWING_NEIGHBORS = "rg_neighbors"; +static const std::string REGION_GROWING_SMOOTHNESS_THRESHOLD = "rg_smoothness_threshold"; +static const std::string REGION_GROWING_CURVATURE_THRESHOLD = "rg_curvature_threshold"; + +static const std::string TRIANGULATION_SEARCH_RADIUS = "tr_search_radius"; +static const std::string TRIANGULATION_MU = "tr_mu"; +static const std::string TRIANGULATION_MAX_NEAREST_NEIGHBORS = "tr_nearest_neighbors"; +static const std::string TRIANGULATION_MAX_SURFACE_ANGLE = "tr_max_surface_angle"; +static const std::string TRIANGULATION_MIN_ANGLE = "tr_min_angle"; +static const std::string TRIANGULATION_MAX_ANGLE = "tr_max_angle"; +static const std::string TRIANGULATION_NORMAL_CONSISTENCY = "tr_normal_consistency"; + +static const std::string PLANE_APROX_REFINEMENT_ENABLED = "pa_enabled"; +static const std::string PLANE_APROX_REFINEMENT_SEG_MAX_ITERATIONS = "pa_seg_max_iterations"; +static const std::string PLANE_APROX_REFINEMENT_SEG_DIST_THRESHOLD = "pa_seg_dist_threshold"; +static const std::string PLANE_APROX_REFINEMENT_SAC_PLANE_DISTANCE = "pa_sac_plane_distance"; +static const std::string PLANE_APROX_REFINEMENT_KDTREE_RADIUS = "pa_kdtree_radius"; + +static const std::string VOXEL_LEAF_SIZE = "voxel_leaf"; + +static const std::string OCCUPANCY_THRESHOLD = "occupancy_threshold"; + +static const std::string MLS_UPSAMPLING_RADIUS = "mls_upsampling_radius"; +static const std::string MLS_SEARCH_RADIUS = "mls_search_radius"; +static const std::string MLS_POINT_DENSITY = "mls_point_density"; + +static const std::string USE_TABLETOP_SEGMENTATION = "use_tabletop_segmentation"; +static const std::string TABLETOP_SEG_DISTANCE_THRESH = "tabletop_seg_distance_thresh"; + +static const std::string MARKER_ALPHA = "marker_alpha"; +static const std::string IGNORE_LARGEST_CLUSTER = "ignore_largest_cluster"; } -class SurfaceDetection { - +class SurfaceDetection +{ public: - SurfaceDetection(); - virtual ~SurfaceDetection(); + SurfaceDetection(); + virtual ~SurfaceDetection(); public: + bool init(); - bool init(); - - bool load_parameters(const std::string& filename); - void save_parameters(const std::string& filename); + bool load_parameters(const std::string& filename); + void save_parameters(const std::string& filename); - bool find_surfaces(); - std::string get_results_summary(); + bool find_surfaces(); + std::string get_results_summary(); - static void mesh_to_marker(const pcl::PolygonMesh &mesh, - visualization_msgs::Marker &marker); + static void mesh_to_marker(const pcl::PolygonMesh& mesh, visualization_msgs::Marker& marker); - // adds point cloud to the occupancy grid, it performs no frame transformation - void add_cloud(Cloud& cloud); - int get_acquired_clouds_count(); + // adds point cloud to the occupancy grid, it performs no frame transformation + void add_cloud(Cloud& cloud); + int get_acquired_clouds_count(); - void clear_results(); + void clear_results(); - // retrieve results - visualization_msgs::MarkerArray get_surface_markers(); - void get_meshes(std::vector& meshes); - std::vector get_surface_clouds(); - void get_full_cloud(Cloud& cloud); - void get_full_cloud(sensor_msgs::PointCloud2 cloud_msg); - void get_region_colored_cloud(CloudRGB& cloud); - void get_region_colored_cloud(sensor_msgs::PointCloud2 &cloud_msg); + // retrieve results + visualization_msgs::MarkerArray get_surface_markers(); + void get_meshes(std::vector& meshes); + std::vector get_surface_clouds(); + void get_full_cloud(Cloud& cloud); + void get_full_cloud(sensor_msgs::PointCloud2 cloud_msg); + void get_region_colored_cloud(CloudRGB& cloud); + void get_region_colored_cloud(sensor_msgs::PointCloud2& cloud_msg); protected: + bool apply_statistical_filter(const Cloud& in, Cloud& out); + bool apply_region_growing_segmentation(const Cloud& in, const Normals& normals, + std::vector& clusters, + CloudRGB& colored_cloud); + bool apply_plane_projection_refinement(const Cloud& candidate_outliers, + const Cloud& surface_cluster, Cloud& projected_cluster); - bool apply_statistical_filter(const Cloud& in,Cloud& out); - bool apply_region_growing_segmentation(const Cloud& in, - const Normals& normals, - std::vector& clusters,CloudRGB& colored_cloud); - bool apply_plane_projection_refinement(const Cloud& candidate_outliers, - const Cloud& surface_cluster,Cloud& projected_cluster); + bool apply_normal_estimation(const Cloud& cloud, Normals& normals); - bool apply_normal_estimation(const Cloud &cloud,Normals& normals); - - bool apply_kdtree_radius_search(const Cloud& query_points,const Cloud& search_points,double radius, - Cloud& close_points); + bool apply_kdtree_radius_search(const Cloud& query_points, const Cloud& search_points, + double radius, Cloud& close_points); - bool apply_voxel_downsampling(Cloud& cloud); + bool apply_voxel_downsampling(Cloud& cloud); - bool apply_mls_surface_smoothing(const Cloud& cloud_in,Cloud& cloud_out,Normals& normals); + bool apply_mls_surface_smoothing(const Cloud& cloud_in, Cloud& cloud_out, Normals& normals); - bool apply_tabletop_segmentation(const Cloud& cloud_in,Cloud& cloud_out); - - /* @brief Finds the best fit plane for the input cloud, reprojects points onto the plane, - * and then calculates the concave hull and triangulates a mesh. - */ - bool apply_planar_reprojection(const Cloud& in, Cloud& out); - bool apply_concave_hull(const Cloud& in, pcl::PolygonMesh& mesh); + bool apply_tabletop_segmentation(const Cloud& cloud_in, Cloud& cloud_out); + /* @brief Finds the best fit plane for the input cloud, reprojects points onto the plane, + * and then calculates the concave hull and triangulates a mesh. + */ + bool apply_planar_reprojection(const Cloud& in, Cloud& out); + bool apply_concave_hull(const Cloud& in, pcl::PolygonMesh& mesh); public: - - // parameters - godel_msgs::SurfaceDetectionParameters params_; + // parameters + godel_msgs::SurfaceDetectionParameters params_; protected: - - // roscpp members - ros::Subscriber point_cloud_subs_; - - // pcl members - pcl::PointCloud::Ptr full_cloud_ptr_; - CloudRGB::Ptr region_colored_cloud_ptr_; - std::vector surface_clouds_; - visualization_msgs::MarkerArray mesh_markers_; - std::vector meshes_; - - - // counter - int acquired_clouds_counter_; - + // roscpp members + ros::Subscriber point_cloud_subs_; + + // pcl members + pcl::PointCloud::Ptr full_cloud_ptr_; + CloudRGB::Ptr region_colored_cloud_ptr_; + std::vector surface_clouds_; + visualization_msgs::MarkerArray mesh_markers_; + std::vector meshes_; + + // counter + int acquired_clouds_counter_; }; - -}} /* namespace godel_surface_detection */ +} +} /* namespace godel_surface_detection */ #endif /* SURFACE_DETECTION_H_ */ diff --git a/godel_surface_detection/include/godel_surface_detection/interactive/interactive_surface_server.h b/godel_surface_detection/include/godel_surface_detection/interactive/interactive_surface_server.h index c5909ea5..4606308b 100644 --- a/godel_surface_detection/include/godel_surface_detection/interactive/interactive_surface_server.h +++ b/godel_surface_detection/include/godel_surface_detection/interactive/interactive_surface_server.h @@ -1,17 +1,17 @@ /* - Copyright Feb 17, 2014 Southwest Research Institute + Copyright Feb 17, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #ifndef INTERACTIVE_SURFACE_SERVER_H_ @@ -26,122 +26,111 @@ namespace interactive_markers { - typedef boost::shared_ptr InteractiveMarkerServerPtr; +typedef boost::shared_ptr InteractiveMarkerServerPtr; } -namespace godel_surface_detection { -namespace interactive { +namespace godel_surface_detection +{ +namespace interactive +{ -namespace params{ - static const std::string PARAMETER_NS = "interactive_surface_server"; +namespace params +{ +static const std::string PARAMETER_NS = "interactive_surface_server"; } -namespace defaults{ - static const std::string MARKER_SERVER_NAME = "surface_marker_server"; - static const std::string FRAME_ID = "world_frame"; - static const std::string MARKER_DESCRIPTION = "surface"; - static const std::string INTERACTIVE_MARKER_NAME = "select surface"; - static const int MAX_TRIANGLES = 20; - static const double ARROW_LENGTH = 0.08f; - static const double ARROW_SHAFT_DIAMETER = 0.02f; - static const double ARROW_HEAD_DIAMETER = 0.04f; - static const double ARROW_HEAD_LENGTH = 0.02f; - static const double ARROW_DISTANCE = 0.02f; - static const double MARKER_ALPHA = 0.6f; +namespace defaults +{ +static const std::string MARKER_SERVER_NAME = "surface_marker_server"; +static const std::string FRAME_ID = "world_frame"; +static const std::string MARKER_DESCRIPTION = "surface"; +static const std::string INTERACTIVE_MARKER_NAME = "select surface"; +static const int MAX_TRIANGLES = 20; +static const double ARROW_LENGTH = 0.08f; +static const double ARROW_SHAFT_DIAMETER = 0.02f; +static const double ARROW_HEAD_DIAMETER = 0.04f; +static const double ARROW_HEAD_LENGTH = 0.02f; +static const double ARROW_DISTANCE = 0.02f; +static const double MARKER_ALPHA = 0.6f; } -class InteractiveSurfaceServer { +class InteractiveSurfaceServer +{ public: - - typedef std::pair Selection; - typedef boost::function SelectionCallback; + typedef std::pair Selection; + typedef boost::function SelectionCallback; public: - - static void mesh_to_marker(const pcl::PolygonMesh &mesh, - visualization_msgs::Marker &marker); - static void marker_to_mesh(const visualization_msgs::Marker &marker, - pcl::PolygonMesh &mesh); + static void mesh_to_marker(const pcl::PolygonMesh& mesh, visualization_msgs::Marker& marker); + static void marker_to_mesh(const visualization_msgs::Marker& marker, pcl::PolygonMesh& mesh); public: - InteractiveSurfaceServer(); - virtual ~InteractiveSurfaceServer(); - - - bool init(); - void run(); - void stop(); - - void add_surface(const visualization_msgs::Marker &marker); - void add_surface(const visualization_msgs::Marker &marker, const geometry_msgs::Pose &pose); - void add_surface(const pcl::PolygonMesh& mesh); - void add_surface(const pcl::PolygonMesh& mesh,const geometry_msgs::Pose& pose); - void add_random_surface_marker(); - void remove_all_surfaces(); - int get_surface_count() - { - return surface_selection_map_.size(); - } - - void get_selected_list(std::vector& list); - void get_selected_surfaces(visualization_msgs::MarkerArray& surfaces); - void get_selected_surfaces(std::vector& meshes); - void add_selection_callback(SelectionCallback &f); - void clear_selection_callbacks(); - void select_all(bool select = true); - void set_selection_flag(std::string marker_name,bool selected); - void toggle_selection_flag(std::string marker_name); - void show(std::string marker_name,bool show = true); - void show_all(bool show_surf); - bool rename_surface(const std::string& old_name, const std::string& new_name); + InteractiveSurfaceServer(); + virtual ~InteractiveSurfaceServer(); + + bool init(); + void run(); + void stop(); + + void add_surface(const visualization_msgs::Marker& marker); + void add_surface(const visualization_msgs::Marker& marker, const geometry_msgs::Pose& pose); + void add_surface(const pcl::PolygonMesh& mesh); + void add_surface(const pcl::PolygonMesh& mesh, const geometry_msgs::Pose& pose); + void add_random_surface_marker(); + void remove_all_surfaces(); + int get_surface_count() { return surface_selection_map_.size(); } + + void get_selected_list(std::vector& list); + void get_selected_surfaces(visualization_msgs::MarkerArray& surfaces); + void get_selected_surfaces(std::vector& meshes); + void add_selection_callback(SelectionCallback& f); + void clear_selection_callbacks(); + void select_all(bool select = true); + void set_selection_flag(std::string marker_name, bool selected); + void toggle_selection_flag(std::string marker_name); + void show(std::string marker_name, bool show = true); + void show_all(bool show_surf); + bool rename_surface(const std::string& old_name, const std::string& new_name); protected: - - interactive_markers::InteractiveMarkerServerPtr marker_server_ptr_; - interactive_markers::MenuHandler menu_handler_; - std::map surface_selection_map_; - std::map meshes_map_; - std::vector selection_callbacks_; + interactive_markers::InteractiveMarkerServerPtr marker_server_ptr_; + interactive_markers::MenuHandler menu_handler_; + std::map surface_selection_map_; + std::map meshes_map_; + std::vector selection_callbacks_; protected: - - void button_marker_callback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); - void menu_marker_callback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); - void invoke_callbacks(); - void create_polygon_marker(visualization_msgs::Marker& marker,int triangles); - void create_arrow_marker(const visualization_msgs::Marker& surface_marker, - visualization_msgs::Marker& arrow_marker); - + void + button_marker_callback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + void menu_marker_callback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + void invoke_callbacks(); + void create_polygon_marker(visualization_msgs::Marker& marker, int triangles); + void create_arrow_marker(const visualization_msgs::Marker& surface_marker, + visualization_msgs::Marker& arrow_marker); public: + std::string marker_name_; + std::string marker_description_; - std::string marker_name_; - std::string marker_description_; - - double arrow_length_; - double arrow_shaft_diameter_; - double arrow_head_diameter_; - double arrow_head_length_; - double arrow_distance_; + double arrow_length_; + double arrow_shaft_diameter_; + double arrow_head_diameter_; + double arrow_head_length_; + double arrow_distance_; protected: - - // menu handling - uint32_t select_entry_id_; - uint32_t unselect_entry_id_; - uint32_t select_all_entry_id_; - uint32_t clear_all_entry_id_; - uint32_t hide_entry_id_; - uint32_t show_all_entry_id_; - - - - // callbacks - interactive_markers::InteractiveMarkerServer::FeedbackCallback button_callback_; - interactive_markers::InteractiveMarkerServer::FeedbackCallback menu_callback_; + // menu handling + uint32_t select_entry_id_; + uint32_t unselect_entry_id_; + uint32_t select_all_entry_id_; + uint32_t clear_all_entry_id_; + uint32_t hide_entry_id_; + uint32_t show_all_entry_id_; + + // callbacks + interactive_markers::InteractiveMarkerServer::FeedbackCallback button_callback_; + interactive_markers::InteractiveMarkerServer::FeedbackCallback menu_callback_; }; } /* namespace interactive */ diff --git a/godel_surface_detection/include/godel_surface_detection/scan/profilimeter_scan.h b/godel_surface_detection/include/godel_surface_detection/scan/profilimeter_scan.h index 80cf4321..be6ef8c5 100644 --- a/godel_surface_detection/include/godel_surface_detection/scan/profilimeter_scan.h +++ b/godel_surface_detection/include/godel_surface_detection/scan/profilimeter_scan.h @@ -1,9 +1,9 @@ /** * This defines some utilities for generating keyence scan paths from a - * polygon boundary. + * polygon boundary. * * The input is a PolygonBoundary which defines the 2d points comprising - * a hole-less surface + * a hole-less surface * * The output is a set of dense points which, when packaged with the associated * pose of the surface, are suitable for trajectory planning. @@ -17,9 +17,9 @@ namespace godel_surface_detection { - std::vector - generateProfilimeterScanPath(const godel_process_path::PolygonBoundary& boundary, - const godel_msgs::ScanPlanParameters& params); +std::vector +generateProfilimeterScanPath(const godel_process_path::PolygonBoundary& boundary, + const godel_msgs::ScanPlanParameters& params); } // end namespace godel_surface_detection diff --git a/godel_surface_detection/include/godel_surface_detection/scan/robot_scan.h b/godel_surface_detection/include/godel_surface_detection/scan/robot_scan.h index 5e6d02a4..f0c21a90 100644 --- a/godel_surface_detection/include/godel_surface_detection/scan/robot_scan.h +++ b/godel_surface_detection/include/godel_surface_detection/scan/robot_scan.h @@ -1,17 +1,17 @@ /* - Copyright Apr 14, 2014 Southwest Research Institute + Copyright Apr 14, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -30,63 +30,63 @@ #ifndef ROBOT_SCAN_H_ #define ROBOT_SCAN_H_ -namespace godel_surface_detection { -namespace scan { +namespace godel_surface_detection +{ +namespace scan +{ typedef boost::shared_ptr MoveGroupPtr; typedef boost::shared_ptr TransformListenerPtr; -class RobotScan { +class RobotScan +{ public: + static const double PLANNING_TIME; + static const double WAIT_MSG_DURATION; + static const double EEF_STEP; + static const double MIN_TRAJECTORY_TIME_STEP; + static const double MIN_JOINT_VELOCITY; - static const double PLANNING_TIME; - static const double WAIT_MSG_DURATION; - static const double EEF_STEP; - static const double MIN_TRAJECTORY_TIME_STEP; - static const double MIN_JOINT_VELOCITY; - -public: - typedef boost::function &cloud)> ScanCallback; public: - RobotScan(); + typedef boost::function& cloud)> ScanCallback; - bool init(); - - bool load_parameters(const std::string& filename); - void save_parameters(const std::string& filename); - - void add_scan_callback(ScanCallback cb); - bool generate_scan_display_trajectory(moveit_msgs::DisplayTrajectory& traj_data); - bool generate_scan_poses(geometry_msgs::PoseArray& poses); - void get_latest_scan_poses(geometry_msgs::PoseArray &poses); - void publish_scan_poses(std::string topic); - MoveGroupPtr get_move_group(); - bool move_to_pose(geometry_msgs::Pose& target_pose); - int scan(bool move_only = false); +public: + RobotScan(); - static void apply_trajectory_parabolic_time_parameterization(robot_trajectory::RobotTrajectory& rt, - moveit_msgs::RobotTrajectory &traj,unsigned int max_iterations=200,double max_time_change_per_it=.6); + bool init(); -protected: + bool load_parameters(const std::string& filename); + void save_parameters(const std::string& filename); + void add_scan_callback(ScanCallback cb); + bool generate_scan_display_trajectory(moveit_msgs::DisplayTrajectory& traj_data); + bool generate_scan_poses(geometry_msgs::PoseArray& poses); + void get_latest_scan_poses(geometry_msgs::PoseArray& poses); + void publish_scan_poses(std::string topic); + MoveGroupPtr get_move_group(); + bool move_to_pose(geometry_msgs::Pose& target_pose); + int scan(bool move_only = false); - // generates circular trajectory above target object - bool create_scan_trajectory(std::vector &scan_poses,moveit_msgs::RobotTrajectory& scan_traj); + static void apply_trajectory_parabolic_time_parameterization( + robot_trajectory::RobotTrajectory& rt, moveit_msgs::RobotTrajectory& traj, + unsigned int max_iterations = 200, double max_time_change_per_it = .6); protected: + // generates circular trajectory above target object + bool create_scan_trajectory(std::vector& scan_poses, + moveit_msgs::RobotTrajectory& scan_traj); - // moveit - MoveGroupPtr move_group_ptr_; - TransformListenerPtr tf_listener_ptr_; - std::vector scan_traj_poses_; - - // scan - std::vector callback_list_; - +protected: + // moveit + MoveGroupPtr move_group_ptr_; + TransformListenerPtr tf_listener_ptr_; + std::vector scan_traj_poses_; -public:// parameters + // scan + std::vector callback_list_; - godel_msgs::RobotScanParameters params_; +public: // parameters + godel_msgs::RobotScanParameters params_; }; } /* namespace detection */ diff --git a/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h b/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h index 5668572e..5fecf721 100644 --- a/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h +++ b/godel_surface_detection/include/godel_surface_detection/services/surface_blending_service.h @@ -16,7 +16,6 @@ #ifndef SURFACE_BLENDING_SERVICE_H #define SURFACE_BLENDING_SERVICE_H - #include #include #include @@ -65,14 +64,16 @@ struct ProcessPathDetails * Associates a name with a visual msgs marker which contains a pose and sequence of points defining * a path */ -struct ProcessPathResult { +struct ProcessPathResult +{ typedef std::pair value_type; std::vector paths; }; /** * Associates a name with a joint trajectory */ -struct ProcessPlanResult { +struct ProcessPlanResult +{ typedef std::pair value_type; std::vector plans; }; @@ -96,42 +97,45 @@ class SurfaceBlendingService void publish_selected_surfaces_changed(); - bool run_robot_scan(visualization_msgs::MarkerArray &surfaces); + bool run_robot_scan(visualization_msgs::MarkerArray& surfaces); - bool find_surfaces(visualization_msgs::MarkerArray &surfaces); + bool find_surfaces(visualization_msgs::MarkerArray& surfaces); void remove_previous_process_plan(); - + /** * The following path generation and planning methods are defined in * src/blending_service_path_generation.cpp */ - bool generate_process_plan(godel_process_path_generation::VisualizeBlendingPlan &process_plan); + bool generate_process_plan(godel_process_path_generation::VisualizeBlendingPlan& process_plan); bool animate_tool_path(); void tool_animation_timer_callback(); - - visualization_msgs::MarkerArray create_tool_markers(const geometry_msgs::Point &pos, - const geometry_msgs::Pose &pose, + + visualization_msgs::MarkerArray create_tool_markers(const geometry_msgs::Point& pos, + const geometry_msgs::Pose& pose, std::string frame_id); // Service callbacks, these components drive this class by signalling events // from the user - bool surface_detection_server_callback(godel_msgs::SurfaceDetection::Request &req, - godel_msgs::SurfaceDetection::Response &res); + bool surface_detection_server_callback(godel_msgs::SurfaceDetection::Request& req, + godel_msgs::SurfaceDetection::Response& res); + + bool select_surface_server_callback(godel_msgs::SelectSurface::Request& req, + godel_msgs::SelectSurface::Response& res); - bool select_surface_server_callback(godel_msgs::SelectSurface::Request &req, - godel_msgs::SelectSurface::Response &res); + bool process_path_server_callback(godel_msgs::ProcessPlanning::Request& req, + godel_msgs::ProcessPlanning::Response& res); - bool process_path_server_callback(godel_msgs::ProcessPlanning::Request &req, - godel_msgs::ProcessPlanning::Response &res); - - bool surface_blend_parameters_server_callback(godel_msgs::SurfaceBlendingParameters::Request &req, - godel_msgs::SurfaceBlendingParameters::Response &res); + bool + surface_blend_parameters_server_callback(godel_msgs::SurfaceBlendingParameters::Request& req, + godel_msgs::SurfaceBlendingParameters::Response& res); // Reads from the surface selection server and generates blend/scan paths for each - godel_surface_detection::TrajectoryLibrary generateMotionLibrary(const godel_msgs::BlendingPlanParameters& blend_params, const godel_msgs::ScanPlanParameters &scan_params); + godel_surface_detection::TrajectoryLibrary + generateMotionLibrary(const godel_msgs::BlendingPlanParameters& blend_params, + const godel_msgs::ScanPlanParameters& scan_params); bool requestBlendPath(const godel_process_path::PolygonBoundaryCollection& boundaries, const geometry_msgs::Pose& boundary_pose, @@ -139,17 +143,18 @@ class SurfaceBlendingService visualization_msgs::Marker& path); bool requestScanPath(const godel_process_path::PolygonBoundaryCollection& boundaries, - const geometry_msgs::Pose& boundary_pose, const godel_msgs::ScanPlanParameters ¶ms, + const geometry_msgs::Pose& boundary_pose, + const godel_msgs::ScanPlanParameters& params, visualization_msgs::Marker& path); - ProcessPathResult generateProcessPath(const std::string& name, - const pcl::PolygonMesh& mesh, - const godel_msgs::BlendingPlanParameters& params, const godel_msgs::ScanPlanParameters &scan_params); + ProcessPathResult generateProcessPath(const std::string& name, const pcl::PolygonMesh& mesh, + const godel_msgs::BlendingPlanParameters& params, + const godel_msgs::ScanPlanParameters& scan_params); - ProcessPlanResult generateProcessPlan(const std::string& name, + ProcessPlanResult generateProcessPlan(const std::string& name, const visualization_msgs::Marker& path, - const godel_msgs::BlendingPlanParameters& params, - const godel_msgs::ScanPlanParameters &scan_params); + const godel_msgs::BlendingPlanParameters& params, + const godel_msgs::ScanPlanParameters& scan_params); bool selectMotionPlanCallback(godel_msgs::SelectMotionPlan::Request& req, godel_msgs::SelectMotionPlan::Response& res); @@ -214,7 +219,8 @@ class SurfaceBlendingService // results godel_msgs::SurfaceDetection::Response latest_surface_detection_results_; ProcessPathDetails process_path_results_; - std::vector > duration_results_; // returned by visualize plan service, needed by trajectory planner + std::vector > + duration_results_; // returned by visualize plan service, needed by trajectory planner // parameters bool publish_region_point_cloud_; diff --git a/godel_surface_detection/include/godel_surface_detection/services/trajectory_library.h b/godel_surface_detection/include/godel_surface_detection/services/trajectory_library.h index 786d73ea..4243816d 100644 --- a/godel_surface_detection/include/godel_surface_detection/services/trajectory_library.h +++ b/godel_surface_detection/include/godel_surface_detection/services/trajectory_library.h @@ -23,7 +23,6 @@ class TrajectoryLibrary private: TrajectoryMap map_; }; - } #endif diff --git a/godel_surface_detection/src/detection/surface_detection.cpp b/godel_surface_detection/src/detection/surface_detection.cpp index f77cb783..00f2fc91 100644 --- a/godel_surface_detection/src/detection/surface_detection.cpp +++ b/godel_surface_detection/src/detection/surface_detection.cpp @@ -1,17 +1,17 @@ /* - Copyright Feb 11, 2014 Southwest Research Institute + Copyright Feb 11, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -39,751 +39,753 @@ #include #include - const static double CONCAVE_HULL_ALPHA = 0.1; const static double PASSTHROUGH_Z_MIN = -0.5; const static double PASSTHROUGH_Z_MAX = 0.5; -namespace godel_surface_detection { namespace detection{ +namespace godel_surface_detection +{ +namespace detection +{ -SurfaceDetection::SurfaceDetection(): - full_cloud_ptr_(new Cloud()), - acquired_clouds_counter_(0) +SurfaceDetection::SurfaceDetection() : full_cloud_ptr_(new Cloud()), acquired_clouds_counter_(0) { - params_.frame_id = defaults::FRAME_ID; - params_.k_search=defaults::K_SEARCH; - params_.meanK=defaults::STATISTICAL_OUTLIER_MEAN; - params_.stdv_threshold=defaults::STATISTICAL_OUTLIER_STDEV_THRESHOLD; - params_.rg_min_cluster_size=defaults::REGION_GROWING_MIN_CLUSTER_SIZE; - params_.rg_max_cluster_size=defaults::REGION_GROWING_MAX_CLUSTER_SIZE; - params_.rg_neightbors=defaults::REGION_GROWING_NEIGHBORS; - params_.rg_smoothness_threshold=defaults::REGION_GROWING_SMOOTHNESS_THRESHOLD; - params_.rg_curvature_threshold=defaults::REGION_GROWING_CURVATURE_THRESHOLD; - params_.tr_search_radius=defaults::TRIANGULATION_SEARCH_RADIUS; - params_.tr_mu=defaults::TRIANGULATION_MU; - params_.tr_max_nearest_neighbors=defaults::TRIANGULATION_MAX_NEAREST_NEIGHBORS; - params_.tr_max_surface_angle=defaults::TRIANGULATION_MAX_SURFACE_ANGLE; - params_.tr_min_angle=defaults::TRIANGULATION_MIN_ANGLE; - params_.tr_max_angle=defaults::TRIANGULATION_MAX_ANGLE; - params_.tr_normal_consistency=defaults::TRIANGULATION_NORMAL_CONSISTENCY; - params_.voxel_leafsize=defaults::VOXEL_LEAF_SIZE; - params_.marker_alpha=defaults::MARKER_ALPHA; - params_.ignore_largest_cluster=defaults::IGNORE_LARGEST_CLUSTER; - params_.occupancy_threshold=defaults::OCCUPANCY_THRESHOLD; - params_.mls_upsampling_radius=defaults::MLS_UPSAMPLING_RADIUS; - params_.mls_point_density=defaults::MLS_POINT_DENSITY; - params_.mls_search_radius=defaults::MLS_SEARCH_RADIUS; - params_.use_tabletop_seg=defaults::USE_TABLETOP_SEGMENTATION; - params_.tabletop_seg_distance_threshold=defaults::TABLETOP_SEG_DISTANCE_THRESH; - - srand(time(NULL)); - clear_results(); + params_.frame_id = defaults::FRAME_ID; + params_.k_search = defaults::K_SEARCH; + params_.meanK = defaults::STATISTICAL_OUTLIER_MEAN; + params_.stdv_threshold = defaults::STATISTICAL_OUTLIER_STDEV_THRESHOLD; + params_.rg_min_cluster_size = defaults::REGION_GROWING_MIN_CLUSTER_SIZE; + params_.rg_max_cluster_size = defaults::REGION_GROWING_MAX_CLUSTER_SIZE; + params_.rg_neightbors = defaults::REGION_GROWING_NEIGHBORS; + params_.rg_smoothness_threshold = defaults::REGION_GROWING_SMOOTHNESS_THRESHOLD; + params_.rg_curvature_threshold = defaults::REGION_GROWING_CURVATURE_THRESHOLD; + params_.tr_search_radius = defaults::TRIANGULATION_SEARCH_RADIUS; + params_.tr_mu = defaults::TRIANGULATION_MU; + params_.tr_max_nearest_neighbors = defaults::TRIANGULATION_MAX_NEAREST_NEIGHBORS; + params_.tr_max_surface_angle = defaults::TRIANGULATION_MAX_SURFACE_ANGLE; + params_.tr_min_angle = defaults::TRIANGULATION_MIN_ANGLE; + params_.tr_max_angle = defaults::TRIANGULATION_MAX_ANGLE; + params_.tr_normal_consistency = defaults::TRIANGULATION_NORMAL_CONSISTENCY; + params_.voxel_leafsize = defaults::VOXEL_LEAF_SIZE; + params_.marker_alpha = defaults::MARKER_ALPHA; + params_.ignore_largest_cluster = defaults::IGNORE_LARGEST_CLUSTER; + params_.occupancy_threshold = defaults::OCCUPANCY_THRESHOLD; + params_.mls_upsampling_radius = defaults::MLS_UPSAMPLING_RADIUS; + params_.mls_point_density = defaults::MLS_POINT_DENSITY; + params_.mls_search_radius = defaults::MLS_SEARCH_RADIUS; + params_.use_tabletop_seg = defaults::USE_TABLETOP_SEGMENTATION; + params_.tabletop_seg_distance_threshold = defaults::TABLETOP_SEG_DISTANCE_THRESH; + + srand(time(NULL)); + clear_results(); } SurfaceDetection::~SurfaceDetection() { - // TODO Auto-generated destructor stub + // TODO Auto-generated destructor stub } bool SurfaceDetection::init() { - full_cloud_ptr_->header.frame_id = params_.frame_id; - acquired_clouds_counter_ = 0; - return true; + full_cloud_ptr_->header.frame_id = params_.frame_id; + acquired_clouds_counter_ = 0; + return true; } void SurfaceDetection::clear_results() { - acquired_clouds_counter_ = 0; - full_cloud_ptr_->clear(); - surface_clouds_.clear(); - mesh_markers_.markers.clear(); - meshes_.clear(); + acquired_clouds_counter_ = 0; + full_cloud_ptr_->clear(); + surface_clouds_.clear(); + mesh_markers_.markers.clear(); + meshes_.clear(); } bool SurfaceDetection::load_parameters(const std::string& filename) { - using godel_param_helpers::loadParam; - using godel_param_helpers::loadBoolParam; - - - if (godel_param_helpers::fromFile(filename, params_)) - { - return true; - } - ros::NodeHandle nh ("~/surface_detection"); - return loadParam(nh, params::FRAME_ID, params_.frame_id)&& - loadParam(nh, params::K_SEARCH, params_.k_search) && - - loadParam(nh, params::STOUTLIER_MEAN, params_.meanK) && - loadParam(nh, params::STOUTLIER_STDEV_THRESHOLD, params_.stdv_threshold) && - - loadParam(nh, params::REGION_GROWING_MIN_CLUSTER_SIZE, params_.rg_min_cluster_size) && - loadParam(nh, params::REGION_GROWING_MAX_CLUSTER_SIZE, params_.rg_max_cluster_size) && - loadParam(nh, params::REGION_GROWING_NEIGHBORS, params_.rg_neightbors) && - loadParam(nh, params::REGION_GROWING_SMOOTHNESS_THRESHOLD, params_.rg_smoothness_threshold) && - loadParam(nh, params::REGION_GROWING_CURVATURE_THRESHOLD, params_.rg_curvature_threshold) && - - loadParam(nh, params::PLANE_APROX_REFINEMENT_SEG_MAX_ITERATIONS, params_.pa_seg_max_iterations) && - loadParam(nh, params::PLANE_APROX_REFINEMENT_SEG_DIST_THRESHOLD, params_.pa_seg_dist_threshold) && - loadParam(nh, params::PLANE_APROX_REFINEMENT_SAC_PLANE_DISTANCE, params_.pa_sac_plane_distance) && - loadParam(nh, params::PLANE_APROX_REFINEMENT_KDTREE_RADIUS, params_.pa_kdtree_radius) && - loadBoolParam(nh, params::PLANE_APROX_REFINEMENT_ENABLED, params_.pa_enabled) && - - loadParam(nh, params::VOXEL_LEAF_SIZE, params_.voxel_leafsize) && - loadParam(nh, params::OCCUPANCY_THRESHOLD, params_.occupancy_threshold) && - - loadParam(nh, params::MLS_UPSAMPLING_RADIUS, params_.mls_upsampling_radius) && - loadParam(nh, params::MLS_POINT_DENSITY, params_.mls_point_density) && - loadParam(nh, params::MLS_SEARCH_RADIUS, params_.mls_search_radius) && - - loadBoolParam(nh, params::USE_TABLETOP_SEGMENTATION, params_.use_tabletop_seg) && - loadParam(nh, params::TABLETOP_SEG_DISTANCE_THRESH, params_.tabletop_seg_distance_threshold) && - loadParam(nh, params::MARKER_ALPHA, params_.marker_alpha) && - loadBoolParam(nh, params::IGNORE_LARGEST_CLUSTER, params_.ignore_largest_cluster); + using godel_param_helpers::loadParam; + using godel_param_helpers::loadBoolParam; + + if (godel_param_helpers::fromFile(filename, params_)) + { + return true; + } + ros::NodeHandle nh("~/surface_detection"); + return loadParam(nh, params::FRAME_ID, params_.frame_id) && + loadParam(nh, params::K_SEARCH, params_.k_search) && + + loadParam(nh, params::STOUTLIER_MEAN, params_.meanK) && + loadParam(nh, params::STOUTLIER_STDEV_THRESHOLD, params_.stdv_threshold) && + + loadParam(nh, params::REGION_GROWING_MIN_CLUSTER_SIZE, params_.rg_min_cluster_size) && + loadParam(nh, params::REGION_GROWING_MAX_CLUSTER_SIZE, params_.rg_max_cluster_size) && + loadParam(nh, params::REGION_GROWING_NEIGHBORS, params_.rg_neightbors) && + loadParam(nh, params::REGION_GROWING_SMOOTHNESS_THRESHOLD, + params_.rg_smoothness_threshold) && + loadParam(nh, params::REGION_GROWING_CURVATURE_THRESHOLD, + params_.rg_curvature_threshold) && + + loadParam(nh, params::PLANE_APROX_REFINEMENT_SEG_MAX_ITERATIONS, + params_.pa_seg_max_iterations) && + loadParam(nh, params::PLANE_APROX_REFINEMENT_SEG_DIST_THRESHOLD, + params_.pa_seg_dist_threshold) && + loadParam(nh, params::PLANE_APROX_REFINEMENT_SAC_PLANE_DISTANCE, + params_.pa_sac_plane_distance) && + loadParam(nh, params::PLANE_APROX_REFINEMENT_KDTREE_RADIUS, params_.pa_kdtree_radius) && + loadBoolParam(nh, params::PLANE_APROX_REFINEMENT_ENABLED, params_.pa_enabled) && + + loadParam(nh, params::VOXEL_LEAF_SIZE, params_.voxel_leafsize) && + loadParam(nh, params::OCCUPANCY_THRESHOLD, params_.occupancy_threshold) && + + loadParam(nh, params::MLS_UPSAMPLING_RADIUS, params_.mls_upsampling_radius) && + loadParam(nh, params::MLS_POINT_DENSITY, params_.mls_point_density) && + loadParam(nh, params::MLS_SEARCH_RADIUS, params_.mls_search_radius) && + + loadBoolParam(nh, params::USE_TABLETOP_SEGMENTATION, params_.use_tabletop_seg) && + loadParam(nh, params::TABLETOP_SEG_DISTANCE_THRESH, + params_.tabletop_seg_distance_threshold) && + loadParam(nh, params::MARKER_ALPHA, params_.marker_alpha) && + loadBoolParam(nh, params::IGNORE_LARGEST_CLUSTER, params_.ignore_largest_cluster); } void SurfaceDetection::save_parameters(const std::string& filename) { - if (!godel_param_helpers::toFile(filename, params_)) - { - ROS_WARN_STREAM("Unable to save surface-detection parameters to: " << filename); - } + if (!godel_param_helpers::toFile(filename, params_)) + { + ROS_WARN_STREAM("Unable to save surface-detection parameters to: " << filename); + } } -void SurfaceDetection::mesh_to_marker(const pcl::PolygonMesh &mesh, - visualization_msgs::Marker &marker) +void SurfaceDetection::mesh_to_marker(const pcl::PolygonMesh& mesh, + visualization_msgs::Marker& marker) { - // color value ranges - static const double color_val_min = 0.5f; - static const double color_val_max = 1.0f; - std_msgs::ColorRGBA color; - color.a = 1; - - // set marker properties - tf::poseTFToMsg(tf::Transform::getIdentity(),marker.pose ); - marker.scale.x = marker.scale.y = marker.scale.z = 1; - marker.type = marker.TRIANGLE_LIST; - marker.action = marker.ADD; - - // create color - color.r = color_val_min + - (static_cast(rand())/static_cast(RAND_MAX)) - * (color_val_max - color_val_min); - color.g = color_val_min + - (static_cast(rand())/static_cast(RAND_MAX)) - * (color_val_max - color_val_min); - color.b = color_val_min + - (static_cast(rand())/static_cast(RAND_MAX)) - * (color_val_max - color_val_min); - marker.color = color; - - - // filling points - Cloud points; - pcl::fromPCLPointCloud2(mesh.cloud,points); - for(int i = 0; i < mesh.polygons.size(); i++) - { - const pcl::Vertices &v = mesh.polygons[i]; - for(int j = 0;j < v.vertices.size(); j++) - { - uint32_t index = v.vertices[j]; - geometry_msgs::Point p; - p.x = points.points[index].x; - p.y = points.points[index].y; - p.z = points.points[index].z; - marker.points.push_back(p); - } - } + // color value ranges + static const double color_val_min = 0.5f; + static const double color_val_max = 1.0f; + std_msgs::ColorRGBA color; + color.a = 1; + + // set marker properties + tf::poseTFToMsg(tf::Transform::getIdentity(), marker.pose); + marker.scale.x = marker.scale.y = marker.scale.z = 1; + marker.type = marker.TRIANGLE_LIST; + marker.action = marker.ADD; + + // create color + color.r = color_val_min + + (static_cast(rand()) / static_cast(RAND_MAX)) * + (color_val_max - color_val_min); + color.g = color_val_min + + (static_cast(rand()) / static_cast(RAND_MAX)) * + (color_val_max - color_val_min); + color.b = color_val_min + + (static_cast(rand()) / static_cast(RAND_MAX)) * + (color_val_max - color_val_min); + marker.color = color; + + // filling points + Cloud points; + pcl::fromPCLPointCloud2(mesh.cloud, points); + for (int i = 0; i < mesh.polygons.size(); i++) + { + const pcl::Vertices& v = mesh.polygons[i]; + for (int j = 0; j < v.vertices.size(); j++) + { + uint32_t index = v.vertices[j]; + geometry_msgs::Point p; + p.x = points.points[index].x; + p.y = points.points[index].y; + p.z = points.points[index].z; + marker.points.push_back(p); + } + } } void SurfaceDetection::add_cloud(Cloud& cloud) { - - (*full_cloud_ptr_)+=cloud; - ROS_INFO_STREAM("Concatenated new cloud to acquired clouds"); - acquired_clouds_counter_++; - ROS_INFO_STREAM("Surface Detection is currently holding "< &meshes) +void SurfaceDetection::get_meshes(std::vector& meshes) { - meshes.insert(meshes.end(),meshes_.begin(),meshes_.end()); + meshes.insert(meshes.end(), meshes_.begin(), meshes_.end()); } -std::vector SurfaceDetection::get_surface_clouds() -{ - return surface_clouds_; -} +std::vector SurfaceDetection::get_surface_clouds() { return surface_clouds_; } std::string SurfaceDetection::get_results_summary() { - std::stringstream ss; - if(surface_clouds_.size() > 0) - { - ss<<"\nNumber of surfaces identified: "<size()<<"}\n"; - } - - } - else - { - ss<<"\nNo surfaces have been found\n"; - } - - return ss.str(); + std::stringstream ss; + if (surface_clouds_.size() > 0) + { + ss << "\nNumber of surfaces identified: " << surface_clouds_.size() << "\n"; + for (int i = 0; i < surface_clouds_.size(); i++) + { + ss << "\t-segment " << i + 1 << " {points: " << surface_clouds_[i]->size() << "}\n"; + } + } + else + { + ss << "\nNo surfaces have been found\n"; + } + + return ss.str(); } void SurfaceDetection::get_full_cloud(Cloud& cloud) { - pcl::copyPointCloud(*full_cloud_ptr_,cloud); - + pcl::copyPointCloud(*full_cloud_ptr_, cloud); } void SurfaceDetection::get_full_cloud(sensor_msgs::PointCloud2 cloud_msg) { - pcl::toROSMsg(*full_cloud_ptr_,cloud_msg); + pcl::toROSMsg(*full_cloud_ptr_, cloud_msg); } -void SurfaceDetection::get_region_colored_cloud(CloudRGB& cloud ) +void SurfaceDetection::get_region_colored_cloud(CloudRGB& cloud) { - pcl::copyPointCloud(*region_colored_cloud_ptr_,cloud); - cloud.header.frame_id = params_.frame_id; + pcl::copyPointCloud(*region_colored_cloud_ptr_, cloud); + cloud.header.frame_id = params_.frame_id; } -void SurfaceDetection::get_region_colored_cloud(sensor_msgs::PointCloud2 &cloud_msg) +void SurfaceDetection::get_region_colored_cloud(sensor_msgs::PointCloud2& cloud_msg) { - pcl::toROSMsg(*region_colored_cloud_ptr_,cloud_msg); - cloud_msg.header.frame_id = params_.frame_id; + pcl::toROSMsg(*region_colored_cloud_ptr_, cloud_msg); + cloud_msg.header.frame_id = params_.frame_id; } bool SurfaceDetection::find_surfaces() { - // main process point cloud - Cloud::Ptr process_cloud_ptr = Cloud::Ptr(new Cloud()); - - - if(full_cloud_ptr_->empty()) - { - return false; - } - else - { - pcl::copyPointCloud(*full_cloud_ptr_,*process_cloud_ptr); - } - - - // reset members - region_colored_cloud_ptr_ = CloudRGB::Ptr(new CloudRGB()); - surface_clouds_.clear(); - mesh_markers_.markers.clear(); - meshes_.clear(); - - // variables to hold intermediate results - Normals::Ptr normals(new Normals()); - std::vector clusters_indices; - std::vector segment_normals; - - // Pass through filter the data to constrain it to our ROI - pcl::PassThrough pass; - pass.setInputCloud(process_cloud_ptr); - pass.setFilterFieldName("z"); - pass.setFilterLimits(PASSTHROUGH_Z_MIN, PASSTHROUGH_Z_MAX); - pass.filter(*process_cloud_ptr); - - ROS_INFO_STREAM("Surface detection processing a cloud containing "<size()<<" points"); - - if(apply_voxel_downsampling(*process_cloud_ptr)) - { - ROS_INFO_STREAM("Voxel downsampling succeeded, downsampled cloud size: "<< - process_cloud_ptr->size()); - } - else - { - ROS_WARN_STREAM("Voxel downsampling failed, cloud size :"<size()); - } - - if(params_.use_tabletop_seg) - { - int count = process_cloud_ptr->size(); - if(apply_tabletop_segmentation(*process_cloud_ptr,*process_cloud_ptr)) - { - ROS_INFO_STREAM("Tabletop segmentation successfully applied, new point count: "<size() - <<", old point cloud: "<size()<<" points"); - if(apply_statistical_filter(*process_cloud_ptr,*process_cloud_ptr)) - { - ROS_INFO_STREAM("Statistical filter completed with "<size()<<" points"); - } - else - { - ROS_ERROR_STREAM("Statistical filter failed"); - return false; - } - - // estimate normals - if(apply_normal_estimation(*process_cloud_ptr,*normals)) - { - ROS_INFO_STREAM("Normal estimation succeeded"); - } - else - { - ROS_ERROR_STREAM("Normal estimation failed"); - return false; - } - - // applying region growing segmentation - Cloud::Ptr ungrouped_cloud_ptr = Cloud::Ptr(new Cloud()); // cloud for storing outliers - pcl::ExtractIndices extract; - pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices()); - if(apply_region_growing_segmentation(*process_cloud_ptr,*normals,clusters_indices, - *region_colored_cloud_ptr_)) - { - - ROS_INFO_STREAM("Region growing succeeded"); - ROS_INFO_STREAM("Total surface clusters found: "<= params_.rg_min_cluster_size) - { - inliers_ptr->indices.insert(inliers_ptr->indices.end(), - indices.indices.begin(),indices.indices.end()); - - pcl::copyPointCloud(*process_cloud_ptr,indices, - *segment_cloud_ptr); - surface_clouds_.push_back(segment_cloud_ptr); - } - } - - // saving all ungrouped points - extract.setInputCloud(process_cloud_ptr); - extract.setIndices(inliers_ptr); - extract.setNegative(true); - extract.filter(*ungrouped_cloud_ptr); - - ROS_INFO_STREAM("Total ungrouped points: "<size()); - ROS_INFO_STREAM("Valid surface clusters kept :"<size() >0 && params_.pa_enabled; i++) - { - - Cloud::Ptr segment_cloud_ptr = surface_clouds_[i]; - int count = segment_cloud_ptr->size(); - - if(apply_plane_projection_refinement(*ungrouped_cloud_ptr,*segment_cloud_ptr,*segment_cloud_ptr)) - { - - ROS_INFO_STREAM("Plane approximation refinement for cluster "<size()<<" ] points"); - } - else - { - ROS_WARN_STREAM("Plane approximation refinement for cluster "<size()<<" ] points"); - } - } - - // applying mls surface smoothing - for(int i =0;i< surface_clouds_.size(); i++) - { - - Normals::Ptr segment_normal_ptr = Normals::Ptr(new Normals()); - Cloud::Ptr segment_cloud_ptr = surface_clouds_[i]; - int count = segment_cloud_ptr->size(); - if(params_.mls_point_density>0 && - apply_mls_surface_smoothing(*segment_cloud_ptr,*segment_cloud_ptr,*segment_normal_ptr)) - { - ROS_INFO_STREAM("Moving least squares smoothing for cluster "<size()<<" ] points"); - } - else - { - ROS_WARN_STREAM("Moving least squares smoothing "<< (params_.mls_point_density>0 ? "failed" :"disabled" ) << - " for cluster "<header.frame_id = params_.frame_id; - segment_normals.push_back(segment_normal_ptr); - } - - ROS_INFO_STREAM("Selected surface clusters (> "< 1) - { - int largest_index = 0; - int largest_size = 0; - for(int i = 0;i < surface_clouds_.size();i++) - { - if(surface_clouds_[i]->points.size() > largest_size) - { - largest_size = surface_clouds_[i]->points.size(); - largest_index = i; - } - } - - ROS_INFO_STREAM("Removing larges cluster from results: cluster index [ "<< - largest_index<<" ], cluster size [ "<header.frame_id; - marker.id = i; - marker.color.a = params_.marker_alpha; - mesh_markers_.markers.push_back(marker); - meshes_.push_back(mesh); - - ROS_INFO_STREAM("Triangulation for surface "<empty()) + { + return false; + } + else + { + pcl::copyPointCloud(*full_cloud_ptr_, *process_cloud_ptr); + } + + // reset members + region_colored_cloud_ptr_ = CloudRGB::Ptr(new CloudRGB()); + surface_clouds_.clear(); + mesh_markers_.markers.clear(); + meshes_.clear(); + + // variables to hold intermediate results + Normals::Ptr normals(new Normals()); + std::vector clusters_indices; + std::vector segment_normals; + + // Pass through filter the data to constrain it to our ROI + pcl::PassThrough pass; + pass.setInputCloud(process_cloud_ptr); + pass.setFilterFieldName("z"); + pass.setFilterLimits(PASSTHROUGH_Z_MIN, PASSTHROUGH_Z_MAX); + pass.filter(*process_cloud_ptr); + + ROS_INFO_STREAM("Surface detection processing a cloud containing " << process_cloud_ptr->size() + << " points"); + + if (apply_voxel_downsampling(*process_cloud_ptr)) + { + ROS_INFO_STREAM( + "Voxel downsampling succeeded, downsampled cloud size: " << process_cloud_ptr->size()); + } + else + { + ROS_WARN_STREAM("Voxel downsampling failed, cloud size :" << process_cloud_ptr->size()); + } + + if (params_.use_tabletop_seg) + { + int count = process_cloud_ptr->size(); + if (apply_tabletop_segmentation(*process_cloud_ptr, *process_cloud_ptr)) + { + ROS_INFO_STREAM("Tabletop segmentation successfully applied, new point count: " + << process_cloud_ptr->size() << ", old point cloud: " << count); + } + else + { + ROS_WARN_STREAM("Tabletop segmentation failed, ignoring results"); + } + } + else + { + ROS_WARN_STREAM("Tabletop segmentation skipped"); + } + + // apply statistical filter + ROS_INFO_STREAM("Statistical filter started with " << process_cloud_ptr->size() << " points"); + if (apply_statistical_filter(*process_cloud_ptr, *process_cloud_ptr)) + { + ROS_INFO_STREAM("Statistical filter completed with " << process_cloud_ptr->size() << " points"); + } + else + { + ROS_ERROR_STREAM("Statistical filter failed"); + return false; + } + + // estimate normals + if (apply_normal_estimation(*process_cloud_ptr, *normals)) + { + ROS_INFO_STREAM("Normal estimation succeeded"); + } + else + { + ROS_ERROR_STREAM("Normal estimation failed"); + return false; + } + + // applying region growing segmentation + Cloud::Ptr ungrouped_cloud_ptr = Cloud::Ptr(new Cloud()); // cloud for storing outliers + pcl::ExtractIndices extract; + pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices()); + if (apply_region_growing_segmentation(*process_cloud_ptr, *normals, clusters_indices, + *region_colored_cloud_ptr_)) + { + + ROS_INFO_STREAM("Region growing succeeded"); + ROS_INFO_STREAM("Total surface clusters found: " << clusters_indices.size()); + + // saving clusters that meet required point count + for (int i = 0; i < clusters_indices.size(); i++) + { + + Cloud::Ptr segment_cloud_ptr = Cloud::Ptr(new Cloud()); + pcl::PointIndices& indices = clusters_indices[i]; + if (indices.indices.size() == 0) + { + continue; + } + + if (indices.indices.size() >= params_.rg_min_cluster_size) + { + inliers_ptr->indices.insert(inliers_ptr->indices.end(), indices.indices.begin(), + indices.indices.end()); + + pcl::copyPointCloud(*process_cloud_ptr, indices, *segment_cloud_ptr); + surface_clouds_.push_back(segment_cloud_ptr); + } + } + + // saving all ungrouped points + extract.setInputCloud(process_cloud_ptr); + extract.setIndices(inliers_ptr); + extract.setNegative(true); + extract.filter(*ungrouped_cloud_ptr); + + ROS_INFO_STREAM("Total ungrouped points: " << ungrouped_cloud_ptr->size()); + ROS_INFO_STREAM("Valid surface clusters kept :" << surface_clouds_.size()); + } + else + { + ROS_ERROR_STREAM("Region growing failed"); + return false; + } + + // applying sac plane segmentation to reintegrate ungrouped points + for (int i = 0; + (i < surface_clouds_.size()) && ungrouped_cloud_ptr->size() > 0 && params_.pa_enabled; i++) + { + + Cloud::Ptr segment_cloud_ptr = surface_clouds_[i]; + int count = segment_cloud_ptr->size(); + + if (apply_plane_projection_refinement(*ungrouped_cloud_ptr, *segment_cloud_ptr, + *segment_cloud_ptr)) + { + + ROS_INFO_STREAM("Plane approximation refinement for cluster " + << i << " completed with [ star: " << count + << ", end: " << segment_cloud_ptr->size() << " ] points"); + } + else + { + ROS_WARN_STREAM("Plane approximation refinement for cluster " + << i << " yielded no matches [ star: " << count + << ", end: " << segment_cloud_ptr->size() << " ] points"); + } + } + + // applying mls surface smoothing + for (int i = 0; i < surface_clouds_.size(); i++) + { + + Normals::Ptr segment_normal_ptr = Normals::Ptr(new Normals()); + Cloud::Ptr segment_cloud_ptr = surface_clouds_[i]; + int count = segment_cloud_ptr->size(); + if (params_.mls_point_density > 0 && + apply_mls_surface_smoothing(*segment_cloud_ptr, *segment_cloud_ptr, *segment_normal_ptr)) + { + ROS_INFO_STREAM("Moving least squares smoothing for cluster " + << i << " completed with [ star: " << count + << ", end: " << segment_cloud_ptr->size() << " ] points"); + } + else + { + ROS_WARN_STREAM("Moving least squares smoothing " + << (params_.mls_point_density > 0 ? "failed" : "disabled") << " for cluster " + << i << ", estimating normals"); + apply_normal_estimation(*segment_cloud_ptr, *segment_normal_ptr); + } + + segment_cloud_ptr->header.frame_id = params_.frame_id; + segment_normals.push_back(segment_normal_ptr); + } + + ROS_INFO_STREAM("Selected surface clusters (> " << params_.rg_min_cluster_size + << " points ) found: " << surface_clouds_.size()); + + if (params_.ignore_largest_cluster && surface_clouds_.size() > 1) + { + int largest_index = 0; + int largest_size = 0; + for (int i = 0; i < surface_clouds_.size(); i++) + { + if (surface_clouds_[i]->points.size() > largest_size) + { + largest_size = surface_clouds_[i]->points.size(); + largest_index = i; + } + } + + ROS_INFO_STREAM("Removing larges cluster from results: cluster index [ " + << largest_index << " ], cluster size [ " << largest_size << " ]"); + surface_clouds_.erase(surface_clouds_.begin() + largest_index); + segment_normals.erase(segment_normals.begin() + largest_index); + } + + // applying fast triangulation + + ROS_INFO_STREAM("Triangulation of surfaces started"); + for (int i = 0; i < surface_clouds_.size(); i++) + { + pcl::PolygonMesh mesh; + visualization_msgs::Marker marker; + + if (!apply_planar_reprojection(*surface_clouds_[i], *surface_clouds_[i])) + { + continue; + } + + if (apply_concave_hull(*surface_clouds_[i], mesh)) + { + mesh_to_marker(mesh, marker); + + // saving other properties + marker.header.frame_id = mesh.header.frame_id = surface_clouds_[i]->header.frame_id; + marker.id = i; + marker.color.a = params_.marker_alpha; + mesh_markers_.markers.push_back(marker); + meshes_.push_back(mesh); + + ROS_INFO_STREAM("Triangulation for surface " << i << " completed"); + } + else + { + ROS_WARN_STREAM("Triangulation for surface " << i << " failed"); + } + } + + ROS_INFO_STREAM("Triangulation of surfaces completed"); + + return true; } -bool SurfaceDetection::apply_statistical_filter(const Cloud& in,Cloud& out) +bool SurfaceDetection::apply_statistical_filter(const Cloud& in, Cloud& out) { - if(params_.meanK ==0) - { - // skip - return true; - } - - pcl::StatisticalOutlierRemoval filter; - Cloud::ConstPtr cloud_ptr = boost::make_shared(in); - filter.setInputCloud(cloud_ptr); - filter.setMeanK(params_.meanK); - filter.setStddevMulThresh(params_.stdv_threshold); - filter.filter(out); - - return !out.empty(); + if (params_.meanK == 0) + { + // skip + return true; + } + + pcl::StatisticalOutlierRemoval filter; + Cloud::ConstPtr cloud_ptr = boost::make_shared(in); + filter.setInputCloud(cloud_ptr); + filter.setMeanK(params_.meanK); + filter.setStddevMulThresh(params_.stdv_threshold); + filter.filter(out); + + return !out.empty(); } -bool SurfaceDetection::apply_normal_estimation(const Cloud &cloud,Normals& normals) +bool SurfaceDetection::apply_normal_estimation(const Cloud& cloud, Normals& normals) { - Cloud::ConstPtr cloud_ptr = boost::make_shared(cloud); - pcl::search::Search::Ptr tree(new pcl::search::KdTree); - pcl::NormalEstimation normal_estimator; - normal_estimator.setViewPoint(0,0,5.0f); - normal_estimator.setSearchMethod(tree); - normal_estimator.setInputCloud(cloud_ptr); - normal_estimator.setKSearch(params_.k_search); - //normal_estimator.setRadiusSearch(0.002f); - normal_estimator.compute(normals); - - return !normals.empty(); + Cloud::ConstPtr cloud_ptr = boost::make_shared(cloud); + pcl::search::Search::Ptr tree(new pcl::search::KdTree); + pcl::NormalEstimation normal_estimator; + normal_estimator.setViewPoint(0, 0, 5.0f); + normal_estimator.setSearchMethod(tree); + normal_estimator.setInputCloud(cloud_ptr); + normal_estimator.setKSearch(params_.k_search); + // normal_estimator.setRadiusSearch(0.002f); + normal_estimator.compute(normals); + + return !normals.empty(); } -bool SurfaceDetection::apply_region_growing_segmentation(const Cloud& in, - const Normals& normals, - std::vector& clusters, - CloudRGB& colored_cloud) +bool SurfaceDetection::apply_region_growing_segmentation(const Cloud& in, const Normals& normals, + std::vector& clusters, + CloudRGB& colored_cloud) { - Cloud::ConstPtr cloud_ptr = boost::make_shared(in); - const Normals::Ptr normals_ptr = boost::make_shared(normals); - pcl::search::Search::Ptr tree(new pcl::search::KdTree); - pcl::RegionGrowing rg; - rg.setMinClusterSize(params_.rg_min_cluster_size); - rg.setMaxClusterSize(params_.rg_max_cluster_size); - rg.setSearchMethod(tree); - rg.setNumberOfNeighbours(params_.rg_neightbors); - rg.setInputCloud(cloud_ptr); - rg.setInputNormals(normals_ptr); - rg.setSmoothnessThreshold(params_.rg_smoothness_threshold); - rg.setCurvatureThreshold(params_.rg_curvature_threshold); - rg.extract(clusters); - - if(rg.getColoredCloud() != 0) - { - pcl::copyPointCloud(*rg.getColoredCloud(),colored_cloud); - } - - - return clusters.size()>0; + Cloud::ConstPtr cloud_ptr = boost::make_shared(in); + const Normals::Ptr normals_ptr = boost::make_shared(normals); + pcl::search::Search::Ptr tree(new pcl::search::KdTree); + pcl::RegionGrowing rg; + rg.setMinClusterSize(params_.rg_min_cluster_size); + rg.setMaxClusterSize(params_.rg_max_cluster_size); + rg.setSearchMethod(tree); + rg.setNumberOfNeighbours(params_.rg_neightbors); + rg.setInputCloud(cloud_ptr); + rg.setInputNormals(normals_ptr); + rg.setSmoothnessThreshold(params_.rg_smoothness_threshold); + rg.setCurvatureThreshold(params_.rg_curvature_threshold); + rg.extract(clusters); + + if (rg.getColoredCloud() != 0) + { + pcl::copyPointCloud(*rg.getColoredCloud(), colored_cloud); + } + + return clusters.size() > 0; } - bool SurfaceDetection::apply_plane_projection_refinement(const Cloud& candidate_outliers, - const Cloud& surface_cluster,Cloud& projected_cluster) + const Cloud& surface_cluster, + Cloud& projected_cluster) { - pcl::ModelCoefficients::Ptr coeff_ptr(new pcl::ModelCoefficients()); - pcl::PointIndices::Ptr plane_inliers_ptr(new pcl::PointIndices()); - pcl::SACSegmentation seg; - - // setting segmentation options - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setMaxIterations(params_.pa_seg_max_iterations); - seg.setDistanceThreshold(params_.pa_seg_dist_threshold); - - // finding coefficients - seg.setInputCloud(surface_cluster.makeShared()); - seg.segment(*plane_inliers_ptr,*coeff_ptr); - - // plane projection and proximity search - pcl::SampleConsensusModelPlane model_plane(candidate_outliers.makeShared()); - Eigen::VectorXf model_coeff(4); - - if(plane_inliers_ptr->indices.size()> 0) - { - // projecting to plane - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (surface_cluster.makeShared()); - proj.setModelCoefficients(coeff_ptr); - proj.filter (projected_cluster); - - // adding near points from outliers - std::vector final_inliers; - Cloud::Ptr inlier_points_ptr = Cloud::Ptr(new Cloud()); - model_plane.setInputCloud(candidate_outliers.makeShared()); - model_coeff<< coeff_ptr->values[0],coeff_ptr->values[1],coeff_ptr->values[2],coeff_ptr->values[3]; - model_plane.selectWithinDistance(model_coeff,params_.pa_sac_plane_distance,final_inliers); - pcl::copyPointCloud(candidate_outliers,final_inliers,*inlier_points_ptr); - - // checking distances to main cluster - if(apply_kdtree_radius_search(*inlier_points_ptr,surface_cluster,params_.pa_kdtree_radius,*inlier_points_ptr)) - { - // adding to output cloud - projected_cluster += *inlier_points_ptr; - } - } - else - { - return false; - } - - return true; + pcl::ModelCoefficients::Ptr coeff_ptr(new pcl::ModelCoefficients()); + pcl::PointIndices::Ptr plane_inliers_ptr(new pcl::PointIndices()); + pcl::SACSegmentation seg; + + // setting segmentation options + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(params_.pa_seg_max_iterations); + seg.setDistanceThreshold(params_.pa_seg_dist_threshold); + + // finding coefficients + seg.setInputCloud(surface_cluster.makeShared()); + seg.segment(*plane_inliers_ptr, *coeff_ptr); + + // plane projection and proximity search + pcl::SampleConsensusModelPlane model_plane(candidate_outliers.makeShared()); + Eigen::VectorXf model_coeff(4); + + if (plane_inliers_ptr->indices.size() > 0) + { + // projecting to plane + pcl::ProjectInliers proj; + proj.setModelType(pcl::SACMODEL_PLANE); + proj.setInputCloud(surface_cluster.makeShared()); + proj.setModelCoefficients(coeff_ptr); + proj.filter(projected_cluster); + + // adding near points from outliers + std::vector final_inliers; + Cloud::Ptr inlier_points_ptr = Cloud::Ptr(new Cloud()); + model_plane.setInputCloud(candidate_outliers.makeShared()); + model_coeff << coeff_ptr->values[0], coeff_ptr->values[1], coeff_ptr->values[2], + coeff_ptr->values[3]; + model_plane.selectWithinDistance(model_coeff, params_.pa_sac_plane_distance, final_inliers); + pcl::copyPointCloud(candidate_outliers, final_inliers, *inlier_points_ptr); + + // checking distances to main cluster + if (apply_kdtree_radius_search(*inlier_points_ptr, surface_cluster, params_.pa_kdtree_radius, + *inlier_points_ptr)) + { + // adding to output cloud + projected_cluster += *inlier_points_ptr; + } + } + else + { + return false; + } + + return true; } -bool SurfaceDetection::apply_kdtree_radius_search(const Cloud& query_points,const Cloud& search_points,double radius, - Cloud& close_points) +bool SurfaceDetection::apply_kdtree_radius_search(const Cloud& query_points, + const Cloud& search_points, double radius, + Cloud& close_points) { - pcl::KdTreeFLANN kdtree; - std::vector all_indices; - std::vector near_indices; - std::vector near_distances; - - kdtree.setInputCloud(search_points.makeShared()); - for(int i = 0; i < query_points.size();i++) - { - const pcl::PointXYZ &p = query_points[i]; - near_indices.clear(); - near_distances.clear(); - if(kdtree.radiusSearch(p,radius,near_indices,near_distances)>0) - { - all_indices.push_back(i); - } - } - - if(all_indices.size()>0) - { - pcl::copyPointCloud(query_points,all_indices,close_points); - return true; - } - else - { - return false; - } - + pcl::KdTreeFLANN kdtree; + std::vector all_indices; + std::vector near_indices; + std::vector near_distances; + + kdtree.setInputCloud(search_points.makeShared()); + for (int i = 0; i < query_points.size(); i++) + { + const pcl::PointXYZ& p = query_points[i]; + near_indices.clear(); + near_distances.clear(); + if (kdtree.radiusSearch(p, radius, near_indices, near_distances) > 0) + { + all_indices.push_back(i); + } + } + + if (all_indices.size() > 0) + { + pcl::copyPointCloud(query_points, all_indices, close_points); + return true; + } + else + { + return false; + } } bool SurfaceDetection::apply_voxel_downsampling(Cloud& cloud) { - if(params_.voxel_leafsize > 0.000001f) - { - pcl::VoxelGrid vg; - double min, max; - vg.getFilterLimits(min,max); - ROS_INFO_STREAM("Voxel downsampling with filter limits min: "< 0.000001f) + { + pcl::VoxelGrid vg; + double min, max; + vg.getFilterLimits(min, max); + ROS_INFO_STREAM("Voxel downsampling with filter limits min: " << min << ", max: " << max); + vg.setInputCloud(cloud.makeShared()); + vg.setLeafSize(params_.voxel_leafsize, params_.voxel_leafsize, params_.voxel_leafsize); + vg.filter(cloud); + } + else + { + ROS_WARN_STREAM("Voxel downsampling leaf too close to 0, skipping."); + } + return true; } -bool SurfaceDetection::apply_mls_surface_smoothing(const Cloud& cloud_in,Cloud& cloud_out,Normals& normals) +bool SurfaceDetection::apply_mls_surface_smoothing(const Cloud& cloud_in, Cloud& cloud_out, + Normals& normals) { - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - pcl::PointCloud mls_points; - pcl::MovingLeastSquares mls; - mls.setInputCloud(boost::make_shared(cloud_in)); - mls.setComputeNormals(true); - mls.setPolynomialFit(true); - - mls.setUpsamplingMethod(pcl::MovingLeastSquares::RANDOM_UNIFORM_DENSITY); - mls.setUpsamplingRadius(params_.mls_upsampling_radius); - mls.setPointDensity(params_.mls_point_density); - - mls.setSearchMethod(tree); - mls.setSearchRadius(params_.mls_search_radius); - mls.process(mls_points); - - bool succeeded = mls_points.size() > 0; - if(succeeded) - { - cloud_out.clear(); - normals.clear(); - pcl::copyPointCloud(mls_points,cloud_out); - pcl::copyPointCloud(mls_points,normals); - } - return succeeded; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); + pcl::PointCloud mls_points; + pcl::MovingLeastSquares mls; + mls.setInputCloud(boost::make_shared(cloud_in)); + mls.setComputeNormals(true); + mls.setPolynomialFit(true); + + mls.setUpsamplingMethod( + pcl::MovingLeastSquares::RANDOM_UNIFORM_DENSITY); + mls.setUpsamplingRadius(params_.mls_upsampling_radius); + mls.setPointDensity(params_.mls_point_density); + + mls.setSearchMethod(tree); + mls.setSearchRadius(params_.mls_search_radius); + mls.process(mls_points); + + bool succeeded = mls_points.size() > 0; + if (succeeded) + { + cloud_out.clear(); + normals.clear(); + pcl::copyPointCloud(mls_points, cloud_out); + pcl::copyPointCloud(mls_points, normals); + } + return succeeded; } -bool SurfaceDetection::apply_tabletop_segmentation(const Cloud& cloud_in,Cloud& cloud_out) +bool SurfaceDetection::apply_tabletop_segmentation(const Cloud& cloud_in, Cloud& cloud_out) { - pcl::ModelCoefficients::Ptr coeff_ptr(new pcl::ModelCoefficients()); - pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices()); - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setMaxIterations (params_.pa_seg_max_iterations); - seg.setDistanceThreshold(params_.tabletop_seg_distance_threshold); - seg.setInputCloud(boost::make_shared(cloud_in)); - seg.segment(*inliers_ptr,*coeff_ptr); - - bool succeeded = inliers_ptr->indices.size() > 0; - if(succeeded) - { - pcl::ExtractIndices extract; - extract.setInputCloud(boost::make_shared(cloud_in)); - extract.setIndices(inliers_ptr); - extract.setNegative(true); - extract.filter(cloud_out); - } - - return succeeded; + pcl::ModelCoefficients::Ptr coeff_ptr(new pcl::ModelCoefficients()); + pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices()); + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(params_.pa_seg_max_iterations); + seg.setDistanceThreshold(params_.tabletop_seg_distance_threshold); + seg.setInputCloud(boost::make_shared(cloud_in)); + seg.segment(*inliers_ptr, *coeff_ptr); + + bool succeeded = inliers_ptr->indices.size() > 0; + if (succeeded) + { + pcl::ExtractIndices extract; + extract.setInputCloud(boost::make_shared(cloud_in)); + extract.setIndices(inliers_ptr); + extract.setNegative(true); + extract.filter(cloud_out); + } + + return succeeded; } bool SurfaceDetection::apply_planar_reprojection(const Cloud& in, Cloud& out) { - pcl::ModelCoefficients::Ptr coeff_ptr(new pcl::ModelCoefficients()); - pcl::PointIndices::Ptr plane_inliers_ptr(new pcl::PointIndices()); - pcl::SACSegmentation seg; - - // setting segmentation options - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); - seg.setMaxIterations(params_.pa_seg_max_iterations); - seg.setDistanceThreshold(params_.tabletop_seg_distance_threshold); - - // finding coefficients - seg.setInputCloud(in.makeShared()); - seg.segment(*plane_inliers_ptr, *coeff_ptr); - - if(plane_inliers_ptr->indices.size() == 0) - { - ROS_WARN_STREAM(__FUNCTION__ << ": Could not segment out plane"); - return false; - } - - // If successful, extract points relevant to the plane - pcl::ExtractIndices extract; - extract.setInputCloud (in.makeShared()); - extract.setIndices (plane_inliers_ptr); - extract.setNegative (false); - extract.filter(out); - - // projecting to plane - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (out.makeShared()); - proj.setModelCoefficients(coeff_ptr); - proj.filter (out); - return true; + pcl::ModelCoefficients::Ptr coeff_ptr(new pcl::ModelCoefficients()); + pcl::PointIndices::Ptr plane_inliers_ptr(new pcl::PointIndices()); + pcl::SACSegmentation seg; + + // setting segmentation options + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(params_.pa_seg_max_iterations); + seg.setDistanceThreshold(params_.tabletop_seg_distance_threshold); + + // finding coefficients + seg.setInputCloud(in.makeShared()); + seg.segment(*plane_inliers_ptr, *coeff_ptr); + + if (plane_inliers_ptr->indices.size() == 0) + { + ROS_WARN_STREAM(__FUNCTION__ << ": Could not segment out plane"); + return false; + } + + // If successful, extract points relevant to the plane + pcl::ExtractIndices extract; + extract.setInputCloud(in.makeShared()); + extract.setIndices(plane_inliers_ptr); + extract.setNegative(false); + extract.filter(out); + + // projecting to plane + pcl::ProjectInliers proj; + proj.setModelType(pcl::SACMODEL_PLANE); + proj.setInputCloud(out.makeShared()); + proj.setModelCoefficients(coeff_ptr); + proj.filter(out); + return true; } bool SurfaceDetection::apply_concave_hull(const Cloud& in, pcl::PolygonMesh& mesh) { - pcl::PolygonMesh::Ptr mesh_ptr (new pcl::PolygonMesh); - pcl::ConcaveHull chull; - chull.setInputCloud (in.makeShared()); - chull.setAlpha (CONCAVE_HULL_ALPHA); - chull.reconstruct (*mesh_ptr); - - // Given a complex concave hull polygon, seperate it into triangles - pcl::EarClipping clipping; - clipping.setInputMesh(mesh_ptr); - clipping.process(mesh); - - return mesh.polygons.size() > 0; + pcl::PolygonMesh::Ptr mesh_ptr(new pcl::PolygonMesh); + pcl::ConcaveHull chull; + chull.setInputCloud(in.makeShared()); + chull.setAlpha(CONCAVE_HULL_ALPHA); + chull.reconstruct(*mesh_ptr); + + // Given a complex concave hull polygon, seperate it into triangles + pcl::EarClipping clipping; + clipping.setInputMesh(mesh_ptr); + clipping.process(mesh); + + return mesh.polygons.size() > 0; } - -}} /* namespace godel_surface_detection */ +} +} /* namespace godel_surface_detection */ diff --git a/godel_surface_detection/src/interactive/interactive_surface_server.cpp b/godel_surface_detection/src/interactive/interactive_surface_server.cpp index 2d06fccf..dbc7f0db 100644 --- a/godel_surface_detection/src/interactive/interactive_surface_server.cpp +++ b/godel_surface_detection/src/interactive/interactive_surface_server.cpp @@ -1,17 +1,17 @@ /* - Copyright Feb 10, 2014 Southwest Research Institute + Copyright Feb 10, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -23,543 +23,545 @@ #include #include -namespace godel_surface_detection { -namespace interactive { - -InteractiveSurfaceServer::InteractiveSurfaceServer() : - marker_name_(defaults::MARKER_SERVER_NAME), - marker_description_(defaults::MARKER_DESCRIPTION), - arrow_distance_(defaults::ARROW_DISTANCE), - arrow_head_diameter_(defaults::ARROW_HEAD_DIAMETER), - arrow_head_length_(defaults::ARROW_HEAD_LENGTH), - arrow_length_(defaults::ARROW_LENGTH), - arrow_shaft_diameter_(defaults::ARROW_SHAFT_DIAMETER) +namespace godel_surface_detection +{ +namespace interactive { - // TODO Auto-generated constructor stub +InteractiveSurfaceServer::InteractiveSurfaceServer() + : marker_name_(defaults::MARKER_SERVER_NAME), marker_description_(defaults::MARKER_DESCRIPTION), + arrow_distance_(defaults::ARROW_DISTANCE), + arrow_head_diameter_(defaults::ARROW_HEAD_DIAMETER), + arrow_head_length_(defaults::ARROW_HEAD_LENGTH), arrow_length_(defaults::ARROW_LENGTH), + arrow_shaft_diameter_(defaults::ARROW_SHAFT_DIAMETER) +{ + // TODO Auto-generated constructor stub } -InteractiveSurfaceServer::~InteractiveSurfaceServer() { - // TODO Auto-generated destructor stub +InteractiveSurfaceServer::~InteractiveSurfaceServer() +{ + // TODO Auto-generated destructor stub } -void InteractiveSurfaceServer::mesh_to_marker(const pcl::PolygonMesh &mesh, - visualization_msgs::Marker &marker) +void InteractiveSurfaceServer::mesh_to_marker(const pcl::PolygonMesh& mesh, + visualization_msgs::Marker& marker) { - // color value ranges - static const double color_val_min = 0.5f; - static const double color_val_max = 1.0f; - std_msgs::ColorRGBA color; - color.a = 1; - - // set marker properties - tf::poseTFToMsg(tf::Transform::getIdentity(),marker.pose ); - marker.scale.x = marker.scale.y = marker.scale.z = 1; - marker.type = marker.TRIANGLE_LIST; - marker.action = marker.ADD; - marker.header.frame_id = mesh.header.frame_id; - - // create color - color.r = color_val_min + - (static_cast(rand())/static_cast(RAND_MAX)) - * (color_val_max - color_val_min); - color.g = color_val_min + - (static_cast(rand())/static_cast(RAND_MAX)) - * (color_val_max - color_val_min); - color.b = color_val_min + - (static_cast(rand())/static_cast(RAND_MAX)) - * (color_val_max - color_val_min); - color.a = 1.0f; - marker.color = color; - - - // filling points - pcl::PointCloud points; - pcl::fromPCLPointCloud2(mesh.cloud,points); - for(int i = 0; i < mesh.polygons.size(); i++) - { - const pcl::Vertices &v = mesh.polygons[i]; - for(int j = 0;j < v.vertices.size(); j++) - { - uint32_t index = v.vertices[j]; - geometry_msgs::Point p; - p.x = points.points[index].x; - p.y = points.points[index].y; - p.z = points.points[index].z; - marker.points.push_back(p); - } - } + // color value ranges + static const double color_val_min = 0.5f; + static const double color_val_max = 1.0f; + std_msgs::ColorRGBA color; + color.a = 1; + + // set marker properties + tf::poseTFToMsg(tf::Transform::getIdentity(), marker.pose); + marker.scale.x = marker.scale.y = marker.scale.z = 1; + marker.type = marker.TRIANGLE_LIST; + marker.action = marker.ADD; + marker.header.frame_id = mesh.header.frame_id; + + // create color + color.r = color_val_min + + (static_cast(rand()) / static_cast(RAND_MAX)) * + (color_val_max - color_val_min); + color.g = color_val_min + + (static_cast(rand()) / static_cast(RAND_MAX)) * + (color_val_max - color_val_min); + color.b = color_val_min + + (static_cast(rand()) / static_cast(RAND_MAX)) * + (color_val_max - color_val_min); + color.a = 1.0f; + marker.color = color; + + // filling points + pcl::PointCloud points; + pcl::fromPCLPointCloud2(mesh.cloud, points); + for (int i = 0; i < mesh.polygons.size(); i++) + { + const pcl::Vertices& v = mesh.polygons[i]; + for (int j = 0; j < v.vertices.size(); j++) + { + uint32_t index = v.vertices[j]; + geometry_msgs::Point p; + p.x = points.points[index].x; + p.y = points.points[index].y; + p.z = points.points[index].z; + marker.points.push_back(p); + } + } } -void InteractiveSurfaceServer::marker_to_mesh(const visualization_msgs::Marker &marker, - pcl::PolygonMesh &mesh) +void InteractiveSurfaceServer::marker_to_mesh(const visualization_msgs::Marker& marker, + pcl::PolygonMesh& mesh) { - pcl::PointCloud points; - for(int i = 0; i points; + for (int i = 0; i < marker.points.size(); i += 3) + { + pcl::Vertices v; + for (int j = 0; j < 3; j++) + { + v.vertices.push_back(i + j); + + pcl::PointXYZ p; + p.x = marker.points[i + j].x; + p.y = marker.points[i + j].y; + p.z = marker.points[i + j].z; + points.points.push_back(p); + } + + mesh.polygons.push_back(v); + } + + pcl::toPCLPointCloud2(points, mesh.cloud); + mesh.header.frame_id = marker.header.frame_id; } bool InteractiveSurfaceServer::init() { - srand(time(NULL)); - return true; + srand(time(NULL)); + return true; } void InteractiveSurfaceServer::run() { - marker_server_ptr_ = interactive_markers::InteractiveMarkerServerPtr( - new interactive_markers::InteractiveMarkerServer(defaults::MARKER_SERVER_NAME,"",false)); - // create callbacks - button_callback_ = interactive_markers::InteractiveMarkerServer::FeedbackCallback( - boost::bind(&InteractiveSurfaceServer::button_marker_callback,this,_1)); - - menu_callback_ = interactive_markers::InteractiveMarkerServer::FeedbackCallback( - boost::bind(&InteractiveSurfaceServer::menu_marker_callback,this,_1)); - - // setup menu handler - select_entry_id_ = menu_handler_.insert("Select",menu_callback_); - unselect_entry_id_ = menu_handler_.insert("Unselect",menu_callback_); - interactive_markers::MenuHandler::EntryHandle submenu_handle = menu_handler_.insert("More Options"); - select_all_entry_id_ = menu_handler_.insert(submenu_handle,"Select All",menu_callback_); - clear_all_entry_id_ = menu_handler_.insert(submenu_handle,"Clear Selections",menu_callback_); - hide_entry_id_ = menu_handler_.insert(submenu_handle,"Hide",menu_callback_); - show_all_entry_id_ = menu_handler_.insert(submenu_handle,"Show All",menu_callback_); - - marker_server_ptr_->applyChanges(); + marker_server_ptr_ = interactive_markers::InteractiveMarkerServerPtr( + new interactive_markers::InteractiveMarkerServer(defaults::MARKER_SERVER_NAME, "", false)); + // create callbacks + button_callback_ = interactive_markers::InteractiveMarkerServer::FeedbackCallback( + boost::bind(&InteractiveSurfaceServer::button_marker_callback, this, _1)); + + menu_callback_ = interactive_markers::InteractiveMarkerServer::FeedbackCallback( + boost::bind(&InteractiveSurfaceServer::menu_marker_callback, this, _1)); + + // setup menu handler + select_entry_id_ = menu_handler_.insert("Select", menu_callback_); + unselect_entry_id_ = menu_handler_.insert("Unselect", menu_callback_); + interactive_markers::MenuHandler::EntryHandle submenu_handle = + menu_handler_.insert("More Options"); + select_all_entry_id_ = menu_handler_.insert(submenu_handle, "Select All", menu_callback_); + clear_all_entry_id_ = menu_handler_.insert(submenu_handle, "Clear Selections", menu_callback_); + hide_entry_id_ = menu_handler_.insert(submenu_handle, "Hide", menu_callback_); + show_all_entry_id_ = menu_handler_.insert(submenu_handle, "Show All", menu_callback_); + + marker_server_ptr_->applyChanges(); } void InteractiveSurfaceServer::stop() { - marker_server_ptr_.reset(); - surface_selection_map_.clear(); + marker_server_ptr_.reset(); + surface_selection_map_.clear(); } -void InteractiveSurfaceServer::set_selection_flag(std::string marker_name,bool selected) +void InteractiveSurfaceServer::set_selection_flag(std::string marker_name, bool selected) { - visualization_msgs::InteractiveMarker int_marker; - if(surface_selection_map_.count(marker_name)>0 && marker_server_ptr_->get(marker_name,int_marker)) - { - surface_selection_map_[marker_name] = selected; - int_marker.controls[1].markers[0].type = selected ? visualization_msgs::Marker::ARROW : visualization_msgs::Marker::SPHERE; - int_marker.controls[1].markers[0].scale.x = selected ? arrow_shaft_diameter_: 0.0001f; - int_marker.controls[1].markers[0].scale.y = selected ? arrow_head_diameter_: 0.0001f; - int_marker.controls[1].markers[0].scale.z = selected ? arrow_head_length_: 0.0001f; - - marker_server_ptr_->insert(int_marker); - invoke_callbacks(); - marker_server_ptr_->applyChanges(); - } + visualization_msgs::InteractiveMarker int_marker; + if (surface_selection_map_.count(marker_name) > 0 && + marker_server_ptr_->get(marker_name, int_marker)) + { + surface_selection_map_[marker_name] = selected; + int_marker.controls[1].markers[0].type = + selected ? visualization_msgs::Marker::ARROW : visualization_msgs::Marker::SPHERE; + int_marker.controls[1].markers[0].scale.x = selected ? arrow_shaft_diameter_ : 0.0001f; + int_marker.controls[1].markers[0].scale.y = selected ? arrow_head_diameter_ : 0.0001f; + int_marker.controls[1].markers[0].scale.z = selected ? arrow_head_length_ : 0.0001f; + + marker_server_ptr_->insert(int_marker); + invoke_callbacks(); + marker_server_ptr_->applyChanges(); + } } -void InteractiveSurfaceServer::show(std::string marker_name,bool show) +void InteractiveSurfaceServer::show(std::string marker_name, bool show) { - visualization_msgs::InteractiveMarker int_marker; - if(surface_selection_map_.count(marker_name)>0 && marker_server_ptr_->get(marker_name,int_marker)) - { - - int_marker.controls[0].markers[0].scale.x =show ? 1: 0.0001f; - int_marker.controls[0].markers[0].scale.y = show ? 1: 0.0001f; - int_marker.controls[0].markers[0].scale.z = show ? 1: 0.0001f; - marker_server_ptr_->insert(int_marker); - set_selection_flag(marker_name,show && surface_selection_map_[marker_name]); - } + visualization_msgs::InteractiveMarker int_marker; + if (surface_selection_map_.count(marker_name) > 0 && + marker_server_ptr_->get(marker_name, int_marker)) + { + + int_marker.controls[0].markers[0].scale.x = show ? 1 : 0.0001f; + int_marker.controls[0].markers[0].scale.y = show ? 1 : 0.0001f; + int_marker.controls[0].markers[0].scale.z = show ? 1 : 0.0001f; + marker_server_ptr_->insert(int_marker); + set_selection_flag(marker_name, show && surface_selection_map_[marker_name]); + } } void InteractiveSurfaceServer::select_all(bool select) { - typedef std::map::iterator SelectionIterator; - for(SelectionIterator i = surface_selection_map_.begin();i!= surface_selection_map_.end();i++) - { - set_selection_flag(i->first,select); - } + typedef std::map::iterator SelectionIterator; + for (SelectionIterator i = surface_selection_map_.begin(); i != surface_selection_map_.end(); i++) + { + set_selection_flag(i->first, select); + } } void InteractiveSurfaceServer::show_all(bool show_surf) { - typedef std::map::iterator SelectionIterator; - for(SelectionIterator i = surface_selection_map_.begin();i!= surface_selection_map_.end();i++) - { - show(i->first,show_surf); - } + typedef std::map::iterator SelectionIterator; + for (SelectionIterator i = surface_selection_map_.begin(); i != surface_selection_map_.end(); i++) + { + show(i->first, show_surf); + } } void InteractiveSurfaceServer::invoke_callbacks() { - for(unsigned int i = 0;i < selection_callbacks_.size();i++) - { - selection_callbacks_[i](); - } + for (unsigned int i = 0; i < selection_callbacks_.size(); i++) + { + selection_callbacks_[i](); + } } void InteractiveSurfaceServer::get_selected_list(std::vector& list) { - std::map::iterator i; - for(i = surface_selection_map_.begin();i != surface_selection_map_.end();i++) - { - if(i->second) - { - list.push_back(i->first); - } - } + std::map::iterator i; + for (i = surface_selection_map_.begin(); i != surface_selection_map_.end(); i++) + { + if (i->second) + { + list.push_back(i->first); + } + } } void InteractiveSurfaceServer::get_selected_surfaces(visualization_msgs::MarkerArray& surfaces) { - std::map::iterator i; - for(i = surface_selection_map_.begin();i != surface_selection_map_.end();i++) - { - if(i->second) - { - visualization_msgs::InteractiveMarker int_marker; - const std::string &marker_name = i->first; - marker_server_ptr_->get(marker_name,int_marker); - surfaces.markers.push_back(int_marker.controls[0].markers[0]); - } - } + std::map::iterator i; + for (i = surface_selection_map_.begin(); i != surface_selection_map_.end(); i++) + { + if (i->second) + { + visualization_msgs::InteractiveMarker int_marker; + const std::string& marker_name = i->first; + marker_server_ptr_->get(marker_name, int_marker); + surfaces.markers.push_back(int_marker.controls[0].markers[0]); + } + } } void InteractiveSurfaceServer::get_selected_surfaces(std::vector& meshes) { - std::map::iterator i; - for(i = surface_selection_map_.begin();i != surface_selection_map_.end();i++) - { - if(i->second) - { - meshes.push_back(meshes_map_[i->first]); - } - } + std::map::iterator i; + for (i = surface_selection_map_.begin(); i != surface_selection_map_.end(); i++) + { + if (i->second) + { + meshes.push_back(meshes_map_[i->first]); + } + } } void InteractiveSurfaceServer::toggle_selection_flag(std::string marker_name) { - if(surface_selection_map_.count(marker_name)>0 ) - { - set_selection_flag(marker_name,!surface_selection_map_[marker_name]); - } + if (surface_selection_map_.count(marker_name) > 0) + { + set_selection_flag(marker_name, !surface_selection_map_[marker_name]); + } } void InteractiveSurfaceServer::button_marker_callback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { - switch(feedback->event_type) - { - case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: - //ROS_INFO_STREAM("marker "<marker_name <<" button control was clicked"); - toggle_selection_flag(feedback->marker_name); - break; - - case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: - break; - } + switch (feedback->event_type) + { + case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: + // ROS_INFO_STREAM("marker "<marker_name <<" button control was clicked"); + toggle_selection_flag(feedback->marker_name); + break; + + case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: + break; + } } void InteractiveSurfaceServer::menu_marker_callback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { - switch(feedback->event_type) - { - case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: -// ROS_INFO_STREAM("marker "<marker_name <<" button control was clicked"); - toggle_selection_flag(feedback->marker_name); - break; - - case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: -// ROS_INFO_STREAM("marker: "<marker_name <<", entry_id: "<< feedback->menu_entry_id -// <<", menu control was clicked"); - - if(feedback->menu_entry_id == select_entry_id_ ) - { - set_selection_flag(feedback->marker_name,true); - return; - } - else if(feedback->menu_entry_id == unselect_entry_id_) - { - set_selection_flag(feedback->marker_name,false); - return; - - } - else if(feedback->menu_entry_id == clear_all_entry_id_) - { - - select_all(false); - return; - - } - else if(feedback->menu_entry_id == select_all_entry_id_) - { - select_all(true); - return; - - } - else if(feedback->menu_entry_id == hide_entry_id_) - { - //ROS_WARN_STREAM("'Hide' menu option has not been implemented yet"); - - show(feedback->marker_name,false); - return; - } - else if(feedback->menu_entry_id == show_all_entry_id_) - { - //ROS_WARN_STREAM("'Show All' menu option has not been implemented yet"); - show_all(true); - marker_server_ptr_->applyChanges(); - return; - } - - break; - } + switch (feedback->event_type) + { + case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK: + // ROS_INFO_STREAM("marker "<marker_name <<" button control was clicked"); + toggle_selection_flag(feedback->marker_name); + break; + + case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT: + // ROS_INFO_STREAM("marker: "<marker_name <<", entry_id: "<< + //feedback->menu_entry_id + // <<", menu control was clicked"); + + if (feedback->menu_entry_id == select_entry_id_) + { + set_selection_flag(feedback->marker_name, true); + return; + } + else if (feedback->menu_entry_id == unselect_entry_id_) + { + set_selection_flag(feedback->marker_name, false); + return; + } + else if (feedback->menu_entry_id == clear_all_entry_id_) + { + + select_all(false); + return; + } + else if (feedback->menu_entry_id == select_all_entry_id_) + { + select_all(true); + return; + } + else if (feedback->menu_entry_id == hide_entry_id_) + { + // ROS_WARN_STREAM("'Hide' menu option has not been implemented yet"); + + show(feedback->marker_name, false); + return; + } + else if (feedback->menu_entry_id == show_all_entry_id_) + { + // ROS_WARN_STREAM("'Show All' menu option has not been implemented yet"); + show_all(true); + marker_server_ptr_->applyChanges(); + return; + } + + break; + } } -void InteractiveSurfaceServer::create_arrow_marker( - const visualization_msgs::Marker& surface_marker,visualization_msgs::Marker& arrow_marker) +void InteractiveSurfaceServer::create_arrow_marker(const visualization_msgs::Marker& surface_marker, + visualization_msgs::Marker& arrow_marker) { - // create temporary point cloud - pcl::PointCloud surface; - surface.width = surface_marker.points.size(); - surface.height = 1; - surface.points.resize(surface_marker.points.size()); - for(int i = 0; i < surface_marker.points.size();i++) - { - surface.points[i].x = surface_marker.points[i].x; - surface.points[i].y = surface_marker.points[i].y; - surface.points[i].z = surface_marker.points[i].z; - } - - // finding bouding box bounds - pcl::PointXYZ min,max; - pcl::getMinMax3D(surface,min,max); - - // create arrow - arrow_marker.type = arrow_marker.ARROW; - arrow_marker.scale.x = arrow_shaft_diameter_; - arrow_marker.scale.y = arrow_head_diameter_; - arrow_marker.scale.z = arrow_head_length_; - arrow_marker.color.r = 0; - arrow_marker.color.g = arrow_marker.color.b = arrow_marker.color.a = 1; - arrow_marker.points.resize(2); - - // start point - arrow_marker.points[1].x = 0.5f*(min.x + max.x); - arrow_marker.points[1].y = 0.5f*(min.y + max.y); - arrow_marker.points[1].z = max.z + arrow_distance_; - - // end point - arrow_marker.points[0].x = arrow_marker.points[1].x; - arrow_marker.points[0].y = arrow_marker.points[1].y; - arrow_marker.points[0].z = arrow_marker.points[1].z + arrow_length_; - + // create temporary point cloud + pcl::PointCloud surface; + surface.width = surface_marker.points.size(); + surface.height = 1; + surface.points.resize(surface_marker.points.size()); + for (int i = 0; i < surface_marker.points.size(); i++) + { + surface.points[i].x = surface_marker.points[i].x; + surface.points[i].y = surface_marker.points[i].y; + surface.points[i].z = surface_marker.points[i].z; + } + + // finding bouding box bounds + pcl::PointXYZ min, max; + pcl::getMinMax3D(surface, min, max); + + // create arrow + arrow_marker.type = arrow_marker.ARROW; + arrow_marker.scale.x = arrow_shaft_diameter_; + arrow_marker.scale.y = arrow_head_diameter_; + arrow_marker.scale.z = arrow_head_length_; + arrow_marker.color.r = 0; + arrow_marker.color.g = arrow_marker.color.b = arrow_marker.color.a = 1; + arrow_marker.points.resize(2); + + // start point + arrow_marker.points[1].x = 0.5f * (min.x + max.x); + arrow_marker.points[1].y = 0.5f * (min.y + max.y); + arrow_marker.points[1].z = max.z + arrow_distance_; + + // end point + arrow_marker.points[0].x = arrow_marker.points[1].x; + arrow_marker.points[0].y = arrow_marker.points[1].y; + arrow_marker.points[0].z = arrow_marker.points[1].z + arrow_length_; } void InteractiveSurfaceServer::add_surface(const pcl::PolygonMesh& mesh, - const geometry_msgs::Pose &pose) + const geometry_msgs::Pose& pose) { - // convert polygon mesh to marker - visualization_msgs::Marker marker; - mesh_to_marker(mesh,marker); - marker.id = meshes_map_.size(); - marker.color.a = defaults::MARKER_ALPHA; - - // create interactive marker - std::stringstream ss; - ss<insert(int_marker,button_callback_); - menu_handler_.apply(*marker_server_ptr_,int_marker.name); - - // save mesh - surface_selection_map_.insert(std::make_pair(int_marker.name,false)); - meshes_map_.insert(std::make_pair(int_marker.name,mesh)); - set_selection_flag(int_marker.name,false); - - // apply changes - marker_server_ptr_->applyChanges(); + // convert polygon mesh to marker + visualization_msgs::Marker marker; + mesh_to_marker(mesh, marker); + marker.id = meshes_map_.size(); + marker.color.a = defaults::MARKER_ALPHA; + + // create interactive marker + std::stringstream ss; + ss << marker_name_ << "_" << surface_selection_map_.size() + 1; + visualization_msgs::InteractiveMarker int_marker; + int_marker.name = ss.str(); + int_marker.pose = pose; + + // create button control + visualization_msgs::InteractiveMarkerControl button_control; + button_control.interaction_mode = button_control.BUTTON; + button_control.markers.push_back(marker); + button_control.name = "button_" + int_marker.name; + button_control.always_visible = true; + + // create seletected arrow marker + visualization_msgs::Marker arrow_marker; + create_arrow_marker(marker, arrow_marker); + visualization_msgs::InteractiveMarkerControl selected_arrow; + selected_arrow.interaction_mode = selected_arrow.FIXED; + selected_arrow.markers.push_back(arrow_marker); + selected_arrow.name = "selected_" + int_marker.name; + selected_arrow.always_visible = true; + + // fill interactive marker + int_marker.controls.push_back(button_control); + int_marker.controls.push_back(selected_arrow); + int_marker.scale = 1; + int_marker.header.frame_id = marker.header.frame_id; + int_marker.description = marker_description_; + + // add marker to server + marker_server_ptr_->insert(int_marker, button_callback_); + menu_handler_.apply(*marker_server_ptr_, int_marker.name); + + // save mesh + surface_selection_map_.insert(std::make_pair(int_marker.name, false)); + meshes_map_.insert(std::make_pair(int_marker.name, mesh)); + set_selection_flag(int_marker.name, false); + + // apply changes + marker_server_ptr_->applyChanges(); } void InteractiveSurfaceServer::remove_all_surfaces() { - surface_selection_map_.clear(); - marker_server_ptr_->clear(); - meshes_map_.clear(); - invoke_callbacks(); - marker_server_ptr_->applyChanges(); + surface_selection_map_.clear(); + marker_server_ptr_->clear(); + meshes_map_.clear(); + invoke_callbacks(); + marker_server_ptr_->applyChanges(); } void InteractiveSurfaceServer::add_surface(const pcl::PolygonMesh& mesh) { - geometry_msgs::Pose pose; - tf::poseTFToMsg(tf::Transform::getIdentity(),pose); - add_surface(mesh,pose); + geometry_msgs::Pose pose; + tf::poseTFToMsg(tf::Transform::getIdentity(), pose); + add_surface(mesh, pose); } -void InteractiveSurfaceServer::add_surface(const visualization_msgs::Marker &marker) +void InteractiveSurfaceServer::add_surface(const visualization_msgs::Marker& marker) { - pcl::PolygonMesh mesh; - marker_to_mesh(marker,mesh); - add_surface(mesh); + pcl::PolygonMesh mesh; + marker_to_mesh(marker, mesh); + add_surface(mesh); } -void InteractiveSurfaceServer::add_surface(const visualization_msgs::Marker &marker, - const geometry_msgs::Pose &pose) +void InteractiveSurfaceServer::add_surface(const visualization_msgs::Marker& marker, + const geometry_msgs::Pose& pose) { - pcl::PolygonMesh mesh; - marker_to_mesh(marker,mesh); - add_surface(mesh,pose); + pcl::PolygonMesh mesh; + marker_to_mesh(marker, mesh); + add_surface(mesh, pose); } -void InteractiveSurfaceServer::add_selection_callback(SelectionCallback &f) +void InteractiveSurfaceServer::add_selection_callback(SelectionCallback& f) { - selection_callbacks_.push_back(f); + selection_callbacks_.push_back(f); } -void InteractiveSurfaceServer::clear_selection_callbacks() -{ - selection_callbacks_.clear(); -} +void InteractiveSurfaceServer::clear_selection_callbacks() { selection_callbacks_.clear(); } void InteractiveSurfaceServer::add_random_surface_marker() { - // create pose - geometry_msgs::Pose pose; - double xy_step = 0.2f; - double z_step = 0.1f; - double pmin = -2; - double pmax = 2; - tfScalar x= (pmax-pmin)*(static_cast(rand())/static_cast(RAND_MAX)) + pmin; - tfScalar y= (pmax-pmin)*(static_cast(rand())/static_cast(RAND_MAX)) + pmin;; - tfScalar z= (pmax-pmin)*(static_cast(rand())/static_cast(RAND_MAX)) + pmin;; - tf::Transform t = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(x,y,z)); - tf::poseTFToMsg(t,pose); - - // create triangle list marker - visualization_msgs::Marker marker; - create_polygon_marker(marker,rand() % defaults::MAX_TRIANGLES + 1); - - add_surface(marker,pose); + // create pose + geometry_msgs::Pose pose; + double xy_step = 0.2f; + double z_step = 0.1f; + double pmin = -2; + double pmax = 2; + tfScalar x = (pmax - pmin) * (static_cast(rand()) / static_cast(RAND_MAX)) + pmin; + tfScalar y = (pmax - pmin) * (static_cast(rand()) / static_cast(RAND_MAX)) + pmin; + ; + tfScalar z = (pmax - pmin) * (static_cast(rand()) / static_cast(RAND_MAX)) + pmin; + ; + tf::Transform t = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(x, y, z)); + tf::poseTFToMsg(t, pose); + + // create triangle list marker + visualization_msgs::Marker marker; + create_polygon_marker(marker, rand() % defaults::MAX_TRIANGLES + 1); + + add_surface(marker, pose); } - -void InteractiveSurfaceServer::create_polygon_marker( - visualization_msgs::Marker& marker,int triangles) +void InteractiveSurfaceServer::create_polygon_marker(visualization_msgs::Marker& marker, + int triangles) { - ROS_INFO_STREAM("Creating polygon of "< points; - points.push_back(p0);points.push_back(p1);points.push_back(p2); - - // adding points to marker - for(unsigned int j = 0 ; j< points.size();j++) - { - geometry_msgs::Point p; - p.x = points[j].getX();p.y = points[j].getY();p.z = points[j].getZ(); - marker.points.push_back(p); - } - - // assigning new points for next triangle - if(rand()%2 == 0) - { - p1 = p2; - } - else - { - p0 = p2; - } - } + ROS_INFO_STREAM("Creating polygon of " << triangles << " triangles"); + + // setting marker properties + marker.type = marker.TRIANGLE_LIST; + marker.scale.x = marker.scale.y = marker.scale.z = 1; + marker.pose.position.x = marker.pose.position.y = marker.pose.position.z = 0; + marker.pose.orientation.x = marker.pose.orientation.y = marker.pose.orientation.z = 0; + marker.pose.orientation.w = 1; + marker.color.r = 0.5f; + marker.color.g = 0.5f; + marker.color.b = 0.8f; + marker.color.a = 0.4f; + + // defining triangle vertices + tf::Vector3 p0(-0.05f, 0, 0); + tf::Vector3 p1(0.05f, 0, 0); + tf::Vector3 p2; + + // rotation transform + tf::Transform rot(tf::Quaternion(tf::Vector3(0, 0, 1), M_PI / 3.0f), tf::Vector3(0, 0, 0)); + + for (int i = 0; i < triangles; i++) + { + // computing last triangle point + p2 = rot * (p1 - p0) + p0; + + // creating temp vector + std::vector points; + points.push_back(p0); + points.push_back(p1); + points.push_back(p2); + + // adding points to marker + for (unsigned int j = 0; j < points.size(); j++) + { + geometry_msgs::Point p; + p.x = points[j].getX(); + p.y = points[j].getY(); + p.z = points[j].getZ(); + marker.points.push_back(p); + } + + // assigning new points for next triangle + if (rand() % 2 == 0) + { + p1 = p2; + } + else + { + p0 = p2; + } + } } -bool InteractiveSurfaceServer::rename_surface(const std::string& old_name, const std::string& new_name) +bool InteractiveSurfaceServer::rename_surface(const std::string& old_name, + const std::string& new_name) { - std::map::iterator select_iter = surface_selection_map_.find(old_name); - std::map::iterator mesh_iter = meshes_map_.find(old_name); - if (select_iter != surface_selection_map_.end() && - mesh_iter != meshes_map_.end()) - { - // The surface exists - bool cache_is_selected = select_iter->second; - pcl::PolygonMesh cache_mesh = mesh_iter->second; - // Insert new item - surface_selection_map_[new_name] = cache_is_selected; - meshes_map_[new_name] = cache_mesh; - // Remove old item - meshes_map_.erase(mesh_iter); - surface_selection_map_.erase(select_iter); - } - else - { - return false; - } + std::map::iterator select_iter = surface_selection_map_.find(old_name); + std::map::iterator mesh_iter = meshes_map_.find(old_name); + if (select_iter != surface_selection_map_.end() && mesh_iter != meshes_map_.end()) + { + // The surface exists + bool cache_is_selected = select_iter->second; + pcl::PolygonMesh cache_mesh = mesh_iter->second; + // Insert new item + surface_selection_map_[new_name] = cache_is_selected; + meshes_map_[new_name] = cache_mesh; + // Remove old item + meshes_map_.erase(mesh_iter); + surface_selection_map_.erase(select_iter); + } + else + { + return false; + } } } /* namespace interactive */ diff --git a/godel_surface_detection/src/nodes/point_cloud_generator_node.cpp b/godel_surface_detection/src/nodes/point_cloud_generator_node.cpp index 7a0ed5b8..ccedc1e3 100644 --- a/godel_surface_detection/src/nodes/point_cloud_generator_node.cpp +++ b/godel_surface_detection/src/nodes/point_cloud_generator_node.cpp @@ -15,7 +15,7 @@ #include // parameters names -const std::string CLOUD_DESCRIPTIONS="point_cloud_descriptions"; +const std::string CLOUD_DESCRIPTIONS = "point_cloud_descriptions"; const std::string FRAME_ID = "frame_id"; // topics @@ -23,250 +23,236 @@ const std::string POINT_CLOUD_TOPIC = "generated_cloud"; // constants - class GeneratePointCloud { - typedef pcl::PointCloud Cloud; - struct Description - { - tf::Vector3 size; - tf::Transform transform; - double resolution; - }; - - public: - GeneratePointCloud() - { - } - - bool init() - { - XmlRpc::XmlRpcValue list; - ros::NodeHandle ph("~"); - - // loading frame id - if(!ph.getParam(FRAME_ID,frame_id_)) - { - ROS_ERROR_STREAM("failed to load frame id"); - } - - - // parameter numeric_fiels - std::map numeric_fields; - numeric_fields.insert(std::make_pair("x",0)); - numeric_fields.insert(std::make_pair("y",0)); - numeric_fields.insert(std::make_pair("z",0)); - numeric_fields.insert(std::make_pair("rx",0)); - numeric_fields.insert(std::make_pair("ry",0)); - numeric_fields.insert(std::make_pair("rz",0)); - numeric_fields.insert(std::make_pair("l",0)); - numeric_fields.insert(std::make_pair("w",0)); - numeric_fields.insert(std::make_pair("h",0)); - numeric_fields.insert(std::make_pair("resolution",0)); - - // loading point cloud descriptions - if(ph.getParam(CLOUD_DESCRIPTIONS,list)) - { - if(list.getType() == XmlRpc::XmlRpcValue::TypeArray) - { - for(int j = 0; j < list.size(); j++) - { - - XmlRpc::XmlRpcValue &entry = list[j]; - - // parse numeric fields - std::map::iterator i; - for(i = numeric_fields.begin() ; i != numeric_fields.end(); i++) - { - if(entry.hasMember(i->first)) - { - double val = static_cast(entry[i->first]); - numeric_fields[i->first] = val; - } - else - { - ROS_ERROR_STREAM("Point Cloud description entry is missing field '"<first<<"'"); - return false; - } - } - - - // populating structure - Description d; - d.size = tf::Vector3(numeric_fields["l"],numeric_fields["w"],numeric_fields["h"]); - d.transform = tf::Transform( - tf::Quaternion(numeric_fields["ry"],numeric_fields["rx"],numeric_fields["rz"]), - tf::Vector3(numeric_fields["x"],numeric_fields["y"],numeric_fields["z"])); - d.resolution = numeric_fields["resolution"]; - - cloud_descriptions_.push_back(d); - } - - } - } - - return true; - } - - void run() - { - ros::NodeHandle nh; - ros::Publisher cloud_publisher = nh.advertise(POINT_CLOUD_TOPIC,1); - - if(init()) - { - genenerate_cloud(); - - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg(full_cloud_,msg); - - ros::Duration loop_duration(0.4f); - while(ros::ok()) - { - msg.header.stamp = ros::Time::now() - loop_duration; - cloud_publisher.publish(msg); - loop_duration.sleep(); - - } - } - - } - - protected: - - - void genenerate_cloud() - { - full_cloud_.clear(); - for(unsigned int i = 0; i < cloud_descriptions_.size() ; i++) - { - Cloud box; - Description &desc = cloud_descriptions_[i]; - create_box(desc,box); - - - // transforming box - Eigen::Affine3d eigen3d; - tf::transformTFToEigen(desc.transform,eigen3d); - pcl::transformPointCloud(box,box,Eigen::Affine3f(eigen3d)); - - // concatenating box - full_cloud_ +=box; - } - - full_cloud_.header.frame_id = frame_id_; - } - - void create_rectangular_patch(tf::Vector3 start,tf::Vector3 end,double res,Cloud& patch) - { - int count_x = (end.x() - start.x())/res + 1; - int count_y = (end.y() - start.y())/res +1; - - pcl::PointXYZ p; - p.z = 0; - patch.resize(count_x * count_y); - int counter = 0; - for(int i = 0; i < count_x ; i++) - { - p.x = start.x() + res*i; - for(int j = 0 ; j < count_y;j++) - { - p.y = start.y() + res*j; - patch.points[counter] = p; - counter++; - } - } - } - - void create_box(const Description& desc,Cloud& box_points) - { - // face points - Cloud top,bottom,front,rear,left,right,temp; - tf::Vector3 start,end; - - // transforms - tf::Transform t; - Eigen::Affine3d eigen3d; - - // ================================ create top and bottom patches =============================== - start = tf::Vector3(-desc.size.x()/2,-desc.size.y()/2,0); - end = tf::Vector3(desc.size.x()/2,desc.size.y()/2,0); - create_rectangular_patch(start,end,desc.resolution,temp); - - // transform cloud to top - t = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(0,0,0.5f*desc.size.z())); - tf::transformTFToEigen(t,eigen3d); - pcl::transformPointCloud(temp,top,Eigen::Affine3f(eigen3d)); - - // transform cloud to bottom - t = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(0,0,0.5f*-desc.size.z())); - tf::transformTFToEigen(t,eigen3d); - pcl::transformPointCloud(temp,bottom,Eigen::Affine3f(eigen3d)); - - // concatenate - box_points += top; - box_points += bottom; - - // ================================ create front and rear patches =============================== - start = tf::Vector3(-desc.size.x()/2,-desc.size.z()/2,0); - end = tf::Vector3(desc.size.x()/2,desc.size.z()/2,0); - create_rectangular_patch(start,end,desc.resolution,temp); - - // transform cloud to front - t = tf::Transform(tf::Quaternion(tf::Vector3(1,0,0),M_PI_2) - ,tf::Vector3(0,0.5f*desc.size.y(),0)); - tf::transformTFToEigen(t,eigen3d); - pcl::transformPointCloud(temp,front,Eigen::Affine3f(eigen3d)); - - // transform cloud to rear - t = tf::Transform(tf::Quaternion(tf::Vector3(1,0,0),M_PI_2) - ,tf::Vector3(0,0.5f*-desc.size.y(),0)); - tf::transformTFToEigen(t,eigen3d); - pcl::transformPointCloud(temp,rear,Eigen::Affine3f(eigen3d)); - - box_points += front; - box_points += rear; - - // ================================ create left and right patches =============================== - start = tf::Vector3(-desc.size.z()/2,-desc.size.y()/2,0); - end = tf::Vector3(desc.size.z()/2,desc.size.y()/2,0); - create_rectangular_patch(start,end,desc.resolution,temp); - - // transform cloud to left - t = tf::Transform(tf::Quaternion(tf::Vector3(0,1,0),M_PI_2) - ,tf::Vector3(0.5f*desc.size.x(),0,0)); - tf::transformTFToEigen(t,eigen3d); - pcl::transformPointCloud(temp,left,Eigen::Affine3f(eigen3d)); - - // transform cloud to right - t = tf::Transform(tf::Quaternion(tf::Vector3(0,1,0),M_PI_2) - ,tf::Vector3(0.5f*-desc.size.x(),0,0)); - tf::transformTFToEigen(t,eigen3d); - pcl::transformPointCloud(temp,right,Eigen::Affine3f(eigen3d)); - - box_points += left; - box_points += right; - - } - - - - protected: - - std::vector cloud_descriptions_; - std::string frame_id_; - Cloud full_cloud_; - float resolution_; - - - + typedef pcl::PointCloud Cloud; + struct Description + { + tf::Vector3 size; + tf::Transform transform; + double resolution; + }; + +public: + GeneratePointCloud() {} + + bool init() + { + XmlRpc::XmlRpcValue list; + ros::NodeHandle ph("~"); + + // loading frame id + if (!ph.getParam(FRAME_ID, frame_id_)) + { + ROS_ERROR_STREAM("failed to load frame id"); + } + + // parameter numeric_fiels + std::map numeric_fields; + numeric_fields.insert(std::make_pair("x", 0)); + numeric_fields.insert(std::make_pair("y", 0)); + numeric_fields.insert(std::make_pair("z", 0)); + numeric_fields.insert(std::make_pair("rx", 0)); + numeric_fields.insert(std::make_pair("ry", 0)); + numeric_fields.insert(std::make_pair("rz", 0)); + numeric_fields.insert(std::make_pair("l", 0)); + numeric_fields.insert(std::make_pair("w", 0)); + numeric_fields.insert(std::make_pair("h", 0)); + numeric_fields.insert(std::make_pair("resolution", 0)); + + // loading point cloud descriptions + if (ph.getParam(CLOUD_DESCRIPTIONS, list)) + { + if (list.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + for (int j = 0; j < list.size(); j++) + { + + XmlRpc::XmlRpcValue& entry = list[j]; + + // parse numeric fields + std::map::iterator i; + for (i = numeric_fields.begin(); i != numeric_fields.end(); i++) + { + if (entry.hasMember(i->first)) + { + double val = static_cast(entry[i->first]); + numeric_fields[i->first] = val; + } + else + { + ROS_ERROR_STREAM("Point Cloud description entry is missing field '" << i->first + << "'"); + return false; + } + } + + // populating structure + Description d; + d.size = tf::Vector3(numeric_fields["l"], numeric_fields["w"], numeric_fields["h"]); + d.transform = tf::Transform( + tf::Quaternion(numeric_fields["ry"], numeric_fields["rx"], numeric_fields["rz"]), + tf::Vector3(numeric_fields["x"], numeric_fields["y"], numeric_fields["z"])); + d.resolution = numeric_fields["resolution"]; + + cloud_descriptions_.push_back(d); + } + } + } + + return true; + } + + void run() + { + ros::NodeHandle nh; + ros::Publisher cloud_publisher = nh.advertise(POINT_CLOUD_TOPIC, 1); + + if (init()) + { + genenerate_cloud(); + + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(full_cloud_, msg); + + ros::Duration loop_duration(0.4f); + while (ros::ok()) + { + msg.header.stamp = ros::Time::now() - loop_duration; + cloud_publisher.publish(msg); + loop_duration.sleep(); + } + } + } + +protected: + void genenerate_cloud() + { + full_cloud_.clear(); + for (unsigned int i = 0; i < cloud_descriptions_.size(); i++) + { + Cloud box; + Description& desc = cloud_descriptions_[i]; + create_box(desc, box); + + // transforming box + Eigen::Affine3d eigen3d; + tf::transformTFToEigen(desc.transform, eigen3d); + pcl::transformPointCloud(box, box, Eigen::Affine3f(eigen3d)); + + // concatenating box + full_cloud_ += box; + } + + full_cloud_.header.frame_id = frame_id_; + } + + void create_rectangular_patch(tf::Vector3 start, tf::Vector3 end, double res, Cloud& patch) + { + int count_x = (end.x() - start.x()) / res + 1; + int count_y = (end.y() - start.y()) / res + 1; + + pcl::PointXYZ p; + p.z = 0; + patch.resize(count_x * count_y); + int counter = 0; + for (int i = 0; i < count_x; i++) + { + p.x = start.x() + res * i; + for (int j = 0; j < count_y; j++) + { + p.y = start.y() + res * j; + patch.points[counter] = p; + counter++; + } + } + } + + void create_box(const Description& desc, Cloud& box_points) + { + // face points + Cloud top, bottom, front, rear, left, right, temp; + tf::Vector3 start, end; + + // transforms + tf::Transform t; + Eigen::Affine3d eigen3d; + + // ================================ create top and bottom patches + // =============================== + start = tf::Vector3(-desc.size.x() / 2, -desc.size.y() / 2, 0); + end = tf::Vector3(desc.size.x() / 2, desc.size.y() / 2, 0); + create_rectangular_patch(start, end, desc.resolution, temp); + + // transform cloud to top + t = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(0, 0, 0.5f * desc.size.z())); + tf::transformTFToEigen(t, eigen3d); + pcl::transformPointCloud(temp, top, Eigen::Affine3f(eigen3d)); + + // transform cloud to bottom + t = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(0, 0, 0.5f * -desc.size.z())); + tf::transformTFToEigen(t, eigen3d); + pcl::transformPointCloud(temp, bottom, Eigen::Affine3f(eigen3d)); + + // concatenate + box_points += top; + box_points += bottom; + + // ================================ create front and rear patches + // =============================== + start = tf::Vector3(-desc.size.x() / 2, -desc.size.z() / 2, 0); + end = tf::Vector3(desc.size.x() / 2, desc.size.z() / 2, 0); + create_rectangular_patch(start, end, desc.resolution, temp); + + // transform cloud to front + t = tf::Transform(tf::Quaternion(tf::Vector3(1, 0, 0), M_PI_2), + tf::Vector3(0, 0.5f * desc.size.y(), 0)); + tf::transformTFToEigen(t, eigen3d); + pcl::transformPointCloud(temp, front, Eigen::Affine3f(eigen3d)); + + // transform cloud to rear + t = tf::Transform(tf::Quaternion(tf::Vector3(1, 0, 0), M_PI_2), + tf::Vector3(0, 0.5f * -desc.size.y(), 0)); + tf::transformTFToEigen(t, eigen3d); + pcl::transformPointCloud(temp, rear, Eigen::Affine3f(eigen3d)); + + box_points += front; + box_points += rear; + + // ================================ create left and right patches + // =============================== + start = tf::Vector3(-desc.size.z() / 2, -desc.size.y() / 2, 0); + end = tf::Vector3(desc.size.z() / 2, desc.size.y() / 2, 0); + create_rectangular_patch(start, end, desc.resolution, temp); + + // transform cloud to left + t = tf::Transform(tf::Quaternion(tf::Vector3(0, 1, 0), M_PI_2), + tf::Vector3(0.5f * desc.size.x(), 0, 0)); + tf::transformTFToEigen(t, eigen3d); + pcl::transformPointCloud(temp, left, Eigen::Affine3f(eigen3d)); + + // transform cloud to right + t = tf::Transform(tf::Quaternion(tf::Vector3(0, 1, 0), M_PI_2), + tf::Vector3(0.5f * -desc.size.x(), 0, 0)); + tf::transformTFToEigen(t, eigen3d); + pcl::transformPointCloud(temp, right, Eigen::Affine3f(eigen3d)); + + box_points += left; + box_points += right; + } + +protected: + std::vector cloud_descriptions_; + std::string frame_id_; + Cloud full_cloud_; + float resolution_; }; -int main(int argc,char** argv) +int main(int argc, char** argv) { - ros::init(argc,argv,"generate_point_cloud_node"); + ros::init(argc, argv, "generate_point_cloud_node"); - GeneratePointCloud g; - g.run(); - return 0; + GeneratePointCloud g; + g.run(); + return 0; } diff --git a/godel_surface_detection/src/nodes/point_cloud_publisher_node.cpp b/godel_surface_detection/src/nodes/point_cloud_publisher_node.cpp index a3fc5fde..434c1c2c 100644 --- a/godel_surface_detection/src/nodes/point_cloud_publisher_node.cpp +++ b/godel_surface_detection/src/nodes/point_cloud_publisher_node.cpp @@ -1,17 +1,17 @@ /* - Copyright Feb 11, 2014 Southwest Research Institute + Copyright Feb 11, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -34,169 +34,161 @@ const float DEFAULT_NOISE_RANGE = 0.005f; const float DEFAULT_PUBLISH_RATE = 0.5f; const std::string CLOUD_TRANSFORM_PARAM = "cloud_transform"; const std::string DEFAULT_FRAME_ID = "world_frame"; -const std::string POINT_CLOUD_TOPIC="sensor_point_cloud"; -const std::string HELP_TEXT="\n-h Help information\n-f \n" - "-n \n-r \n-i "; +const std::string POINT_CLOUD_TOPIC = "sensor_point_cloud"; +const std::string HELP_TEXT = "\n-h Help information\n-f \n" + "-n \n-r \n-i "; typedef pcl::PointCloud CloudRGBA; typedef pcl::PointCloud CloudRGB; -bool read_transform(ros::NodeHandle &nh,std::string param_name,tf::Transform &t) +bool read_transform(ros::NodeHandle& nh, std::string param_name, tf::Transform& t) { - XmlRpc::XmlRpcValue val; - double x,y,z,rx,ry,rz; - bool succeeded = nh.getParam(param_name,val); - if(succeeded) - { - x = val["x"].getType()==val.TypeDouble ? static_cast(val["x"]) : 0; - y = val["y"].getType()==val.TypeDouble ? static_cast(val["y"]): 0; - z = val["z"].getType()==val.TypeDouble ? static_cast(val["z"]): 0; - rx = val["rx"].getType()==val.TypeDouble ? static_cast(val["rx"]): 0; - ry = val["ry"].getType()==val.TypeDouble ? static_cast(val["ry"]): 0; - rz = val["rz"].getType()==val.TypeDouble ? static_cast(val["rz"]): 0; - - t.setOrigin(tf::Vector3(x,y,z)); - t.getBasis().setRPY(rx,ry,rz); - } - - return succeeded; + XmlRpc::XmlRpcValue val; + double x, y, z, rx, ry, rz; + bool succeeded = nh.getParam(param_name, val); + if (succeeded) + { + x = val["x"].getType() == val.TypeDouble ? static_cast(val["x"]) : 0; + y = val["y"].getType() == val.TypeDouble ? static_cast(val["y"]) : 0; + z = val["z"].getType() == val.TypeDouble ? static_cast(val["z"]) : 0; + rx = val["rx"].getType() == val.TypeDouble ? static_cast(val["rx"]) : 0; + ry = val["ry"].getType() == val.TypeDouble ? static_cast(val["ry"]) : 0; + rz = val["rz"].getType() == val.TypeDouble ? static_cast(val["rz"]) : 0; + + t.setOrigin(tf::Vector3(x, y, z)); + t.getBasis().setRPY(rx, ry, rz); + } + + return succeeded; } - -int main(int argc,char** argv) +int main(int argc, char** argv) { - ros::init(argc,argv,"point_cloud_publisher_node"); - - // point cloud publisher - ros::NodeHandle nh; - ros::NodeHandle ph("~"); // private handle for parameter reading - ros::Publisher point_cloud_pub = nh.advertise( - POINT_CLOUD_TOPIC,1); - - // arguments - path file_path; - float noise_range = DEFAULT_NOISE_RANGE; - float rate = DEFAULT_PUBLISH_RATE; - std::string frame_id = DEFAULT_FRAME_ID; - - // point clouds - CloudRGB::Ptr cloud_ptr(new CloudRGB()); - CloudRGB::Ptr noise_cloud_ptr(new CloudRGB()); - - if(argc > 1) - { - if(pcl::console::find_switch(argc,argv,"-h")) - { - ROS_INFO_STREAM(HELP_TEXT); - return 0; - } - - if(pcl::console::find_switch(argc,argv,"-f")) - { - // parsing arguments - std::string fpath; - pcl::console::parse(argc,argv,"-f",fpath); - file_path = path(ros::package::getPath("godel_surface_detection") + - "/"+ fpath); - - if(exists(file_path)) - { - ROS_INFO_STREAM("Found file: "<points.size()<<" points"); - } - - // publishing - ros::Duration loop_time(rate); - tf::Transform cloud_transform = tf::Transform::getIdentity(); - srand(time(NULL)); - float noise; - - ROS_INFO_STREAM("Publishing cloud "); - while(ros::ok()) - { - - // adding noise - pcl::copyPointCloud(*cloud_ptr,*noise_cloud_ptr); - for(int i = 0; i < noise_cloud_ptr->points.size();i++) - { - noise = float(rand())/float(RAND_MAX); // <0,1> - noise = -noise_range*0.5f + noise*noise_range; - pcl::PointXYZRGB &p = noise_cloud_ptr->points[i]; - noise_cloud_ptr->points[i].x = p.x + noise; - noise_cloud_ptr->points[i].y = p.y + noise; - noise_cloud_ptr->points[i].z = p.z + noise; - } - - // transforming point_cloud - if(read_transform(ph,CLOUD_TRANSFORM_PARAM,cloud_transform)) - { - Eigen::Affine3d eigen_transform; - tf::poseTFToEigen(cloud_transform,eigen_transform); - pcl::transformPointCloud(*noise_cloud_ptr,*noise_cloud_ptr,(Eigen::Affine3f)(eigen_transform)); - } - - // convert to msg - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg(*noise_cloud_ptr,msg); - - msg.header.frame_id = msg.header.frame_id.empty() ? frame_id:msg.header.frame_id; - point_cloud_pub.publish(msg); - - loop_time.sleep(); - } - - return 0; + ros::init(argc, argv, "point_cloud_publisher_node"); + + // point cloud publisher + ros::NodeHandle nh; + ros::NodeHandle ph("~"); // private handle for parameter reading + ros::Publisher point_cloud_pub = nh.advertise(POINT_CLOUD_TOPIC, 1); + + // arguments + path file_path; + float noise_range = DEFAULT_NOISE_RANGE; + float rate = DEFAULT_PUBLISH_RATE; + std::string frame_id = DEFAULT_FRAME_ID; + + // point clouds + CloudRGB::Ptr cloud_ptr(new CloudRGB()); + CloudRGB::Ptr noise_cloud_ptr(new CloudRGB()); + + if (argc > 1) + { + if (pcl::console::find_switch(argc, argv, "-h")) + { + ROS_INFO_STREAM(HELP_TEXT); + return 0; + } + + if (pcl::console::find_switch(argc, argv, "-f")) + { + // parsing arguments + std::string fpath; + pcl::console::parse(argc, argv, "-f", fpath); + file_path = path(ros::package::getPath("godel_surface_detection") + "/" + fpath); + + if (exists(file_path)) + { + ROS_INFO_STREAM("Found file: " << file_path.c_str()); + } + else + { + ROS_ERROR_STREAM("Must supply valid pcd file name that exists in the package directory"); + return 0; + } + } + else + { + ROS_ERROR_STREAM("-f argument not found. Must supply pcd file"); + return 0; + } + + if (pcl::console::find_switch(argc, argv, "-n")) + { + pcl::console::parse(argc, argv, "-n", noise_range); + } + + if (pcl::console::find_switch(argc, argv, "-r")) + { + pcl::console::parse(argc, argv, "-r", rate); + } + + if (pcl::console::find_switch(argc, argv, "-i")) + { + pcl::console::parse(argc, argv, "-i", frame_id); + } + + ROS_INFO_STREAM("Using noise " << noise_range); + ROS_INFO_STREAM("Using rate " << rate); + ROS_INFO_STREAM("Using frame_id " << frame_id); + } + else + { + ROS_ERROR_STREAM("Must supply valid pcd file name that exists in the package directory"); + ROS_ERROR_STREAM(HELP_TEXT); + return 0; + } + + // reading pcd file + if (pcl::io::loadPCDFile(std::string(file_path.c_str()), *cloud_ptr) == -1) + { + ROS_ERROR_STREAM("Failed to read pcd file"); + return 0; + } + else + { + ROS_INFO_STREAM("Successfully read pcd file with " << cloud_ptr->points.size() << " points"); + } + + // publishing + ros::Duration loop_time(rate); + tf::Transform cloud_transform = tf::Transform::getIdentity(); + srand(time(NULL)); + float noise; + + ROS_INFO_STREAM("Publishing cloud "); + while (ros::ok()) + { + + // adding noise + pcl::copyPointCloud(*cloud_ptr, *noise_cloud_ptr); + for (int i = 0; i < noise_cloud_ptr->points.size(); i++) + { + noise = float(rand()) / float(RAND_MAX); // <0,1> + noise = -noise_range * 0.5f + noise * noise_range; + pcl::PointXYZRGB& p = noise_cloud_ptr->points[i]; + noise_cloud_ptr->points[i].x = p.x + noise; + noise_cloud_ptr->points[i].y = p.y + noise; + noise_cloud_ptr->points[i].z = p.z + noise; + } + + // transforming point_cloud + if (read_transform(ph, CLOUD_TRANSFORM_PARAM, cloud_transform)) + { + Eigen::Affine3d eigen_transform; + tf::poseTFToEigen(cloud_transform, eigen_transform); + pcl::transformPointCloud(*noise_cloud_ptr, *noise_cloud_ptr, + (Eigen::Affine3f)(eigen_transform)); + } + + // convert to msg + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(*noise_cloud_ptr, msg); + + msg.header.frame_id = msg.header.frame_id.empty() ? frame_id : msg.header.frame_id; + point_cloud_pub.publish(msg); + + loop_time.sleep(); + } + + return 0; } - - - diff --git a/godel_surface_detection/src/nodes/robot_scan_node.cpp b/godel_surface_detection/src/nodes/robot_scan_node.cpp index 6f674b85..4dd2b6ca 100644 --- a/godel_surface_detection/src/nodes/robot_scan_node.cpp +++ b/godel_surface_detection/src/nodes/robot_scan_node.cpp @@ -1,17 +1,17 @@ /* - Copyright Apr 15, 2014 Southwest Research Institute + Copyright Apr 15, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -41,153 +41,146 @@ const bool DEFAULT_MOVE_ONLY_MODE = true; struct CmdArgs { - bool move_mode_only_; + bool move_mode_only_; }; // functions -bool parse_arguments(int argc,char** argv,CmdArgs& args) +bool parse_arguments(int argc, char** argv, CmdArgs& args) { - //filling defaults - args.move_mode_only_ = true; - - if(argc > 1) - { - if(pcl::console::find_switch(argc,argv,"-h")) - { - ROS_INFO_STREAM(HELP_TEXT); - return false; - } - - if(pcl::console::find_switch(argc,argv,"-m") && pcl::console::parse(argc,argv,"-m",args.move_mode_only_)>=0) - { - ROS_INFO_STREAM("arg 'move mode only (-m)': "<<(args.move_mode_only_ ? "true" : "false")); - } - - } - - return true; + // filling defaults + args.move_mode_only_ = true; + + if (argc > 1) + { + if (pcl::console::find_switch(argc, argv, "-h")) + { + ROS_INFO_STREAM(HELP_TEXT); + return false; + } + + if (pcl::console::find_switch(argc, argv, "-m") && + pcl::console::parse(argc, argv, "-m", args.move_mode_only_) >= 0) + { + ROS_INFO_STREAM("arg 'move mode only (-m)': " << (args.move_mode_only_ ? "true" : "false")); + } + } + + return true; } bool init() { - // load parameters for all objects - bool succeeded = SurfDetect.load_parameters("~/surface_detection") && SurfServer.load_parameters("") - && RobotScan.load_parameters("~/robot_scan"); - - if(succeeded) - { - ROS_INFO_STREAM("Parameters loaded"); - ROS_INFO_STREAM("Robot Scan Parameters: \n"<( - SEGMENTS_CLOUD_TOPIC,1); - - // parsing arguments - CmdArgs args; - if(!parse_arguments(argc,argv,args)) - { - return 0; - } - - // initializing all objects - if(init()) - { - - // moving to home position - RobotScan.get_move_group()->setNamedTarget(HOME_POSITION); - if(!RobotScan.get_move_group()->move()) - { - ROS_ERROR_STREAM("Robot failed to move home"); - return 0; - } - - // publish poses - RobotScan.publish_scan_poses(DISPLAY_TRAJECTORY_TOPIC); - - // moving through each pose (do not scan) - int reached_points = RobotScan.scan(args.move_mode_only_); - - ROS_INFO_STREAM("Scan points reached: "<setNamedTarget(HOME_POSITION); - if(!RobotScan.get_move_group()->move()) - { - ROS_ERROR_STREAM("Robot failed to move home"); - return 0; - } - - // finding surfaces - if(SurfDetect.find_surfaces()) - { - ROS_INFO_STREAM("Publishing segments visuals"); - sensor_msgs::PointCloud2 cloud_msg; - std::vector meshes; - SurfDetect.get_meshes(meshes); - SurfDetect.get_region_colored_cloud(cloud_msg); - - // adding markers to server - for(int i =0;i < meshes.size();i++) - { - SurfServer.add_surface(meshes[i]); - } - - ros::Duration loop_rate(0.5f); - while(ros::ok() ) - { - point_cloud_publisher.publish(cloud_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - - point_cloud_publisher.shutdown(); - } - else - { - ROS_WARN_STREAM("No surfaces were found, exiting"); - } - - } - else - { - ROS_ERROR_STREAM("Robot scan object did not initialized property"); - } - - spinner.stop(); - - - return 0; + ros::init(argc, argv, "RobotScan_node"); + ros::AsyncSpinner spinner(2); + spinner.start(); + ros::NodeHandle nh; + + // publishers + ros::Publisher point_cloud_publisher = + nh.advertise(SEGMENTS_CLOUD_TOPIC, 1); + + // parsing arguments + CmdArgs args; + if (!parse_arguments(argc, argv, args)) + { + return 0; + } + + // initializing all objects + if (init()) + { + + // moving to home position + RobotScan.get_move_group()->setNamedTarget(HOME_POSITION); + if (!RobotScan.get_move_group()->move()) + { + ROS_ERROR_STREAM("Robot failed to move home"); + return 0; + } + + // publish poses + RobotScan.publish_scan_poses(DISPLAY_TRAJECTORY_TOPIC); + + // moving through each pose (do not scan) + int reached_points = RobotScan.scan(args.move_mode_only_); + + ROS_INFO_STREAM("Scan points reached: " << reached_points); + + // moving back to home position + RobotScan.get_move_group()->setNamedTarget(HOME_POSITION); + if (!RobotScan.get_move_group()->move()) + { + ROS_ERROR_STREAM("Robot failed to move home"); + return 0; + } + + // finding surfaces + if (SurfDetect.find_surfaces()) + { + ROS_INFO_STREAM("Publishing segments visuals"); + sensor_msgs::PointCloud2 cloud_msg; + std::vector meshes; + SurfDetect.get_meshes(meshes); + SurfDetect.get_region_colored_cloud(cloud_msg); + + // adding markers to server + for (int i = 0; i < meshes.size(); i++) + { + SurfServer.add_surface(meshes[i]); + } + + ros::Duration loop_rate(0.5f); + while (ros::ok()) + { + point_cloud_publisher.publish(cloud_msg); + ros::spinOnce(); + loop_rate.sleep(); + } + + point_cloud_publisher.shutdown(); + } + else + { + ROS_WARN_STREAM("No surfaces were found, exiting"); + } + } + else + { + ROS_ERROR_STREAM("Robot scan object did not initialized property"); + } + + spinner.stop(); + + return 0; } - - - - diff --git a/godel_surface_detection/src/nodes/surface_detection_node.cpp b/godel_surface_detection/src/nodes/surface_detection_node.cpp index 9ae10f98..9318d249 100644 --- a/godel_surface_detection/src/nodes/surface_detection_node.cpp +++ b/godel_surface_detection/src/nodes/surface_detection_node.cpp @@ -1,17 +1,17 @@ /* - Copyright Feb 12, 2014 Southwest Research Institute + Copyright Feb 12, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -24,17 +24,15 @@ using namespace godel_surface_detection::detection; -const float DEFAULT_ACQUISITION_TIME = 2.0f; //second +const float DEFAULT_ACQUISITION_TIME = 2.0f; // second const std::string DEFAULT_FRAME_ID = "world_frame"; -const std::string POINT_CLOUD_TOPIC="sensor_point_cloud"; +const std::string POINT_CLOUD_TOPIC = "sensor_point_cloud"; const std::string SEGMENTS_CLOUD_TOPIC = "segments_cloud"; const std::string MARKER_ARRAY_TOPIC = "segment_markers"; const std::string NODE_NAME = "surface_detection_node"; -const std::string HELP_TEXT="\n" + NODE_NAME + " help:\n" + - "-h help menu\n" + - "-a \n" + - "-i \n"; +const std::string HELP_TEXT = "\n" + NODE_NAME + " help:\n" + "-h help menu\n" + + "-a \n" + "-i \n"; // surface detection instance godel_surface_detection::detection::SurfaceDetection SurfDetect; @@ -42,162 +40,156 @@ godel_surface_detection::detection::SurfaceDetection SurfDetect; // marker server instance godel_surface_detection::interactive::InteractiveSurfaceServer SurfServer; - void point_cloud_subscriber(const sensor_msgs::PointCloud2ConstPtr msg) { - static tf::TransformListener tf_listener; - - // convert to message to point cloud - Cloud::Ptr new_cloud_ptr(new Cloud()); - pcl::fromROSMsg(*msg,*new_cloud_ptr); - - // removed nans - std::vector index; - pcl::removeNaNFromPointCloud(*new_cloud_ptr,*new_cloud_ptr,index); - - // transform to frame id - tf::StampedTransform source_to_target_tf; - if(SurfDetect.params_.frame_id.compare(msg->header.frame_id) !=0) - { - ROS_INFO_STREAM("Source cloud with frame id '"<header.frame_id<<"' will be transformed to frame id: '" - <header.frame_id, - ros::Time::now() - ros::Duration(0.2f),source_to_target_tf); - pcl_ros::transformPointCloud(*new_cloud_ptr,*new_cloud_ptr,source_to_target_tf); - } - catch(tf::LookupException &e) - { - ROS_ERROR_STREAM("Transform lookup error, using source frame id '"<< msg->header.frame_id<<"'"); - SurfDetect.params_.frame_id= msg->header.frame_id; - } - catch(tf::ExtrapolationException &e) - { - ROS_ERROR_STREAM("Transform lookup error, using source frame id '"<< msg->header.frame_id<<"'"); - SurfDetect.params_.frame_id = msg->header.frame_id; - } - - } - else - { - ROS_INFO_STREAM("Source cloud is already in frame id '"<header.frame_id<<", skipping transform"); - } - - // add point cloud - SurfDetect.add_cloud(*new_cloud_ptr); + static tf::TransformListener tf_listener; + + // convert to message to point cloud + Cloud::Ptr new_cloud_ptr(new Cloud()); + pcl::fromROSMsg(*msg, *new_cloud_ptr); + + // removed nans + std::vector index; + pcl::removeNaNFromPointCloud(*new_cloud_ptr, *new_cloud_ptr, index); + + // transform to frame id + tf::StampedTransform source_to_target_tf; + if (SurfDetect.params_.frame_id.compare(msg->header.frame_id) != 0) + { + ROS_INFO_STREAM("Source cloud with frame id '" << msg->header.frame_id + << "' will be transformed to frame id: '" + << SurfDetect.params_.frame_id << "'"); + try + { + tf_listener.lookupTransform(SurfDetect.params_.frame_id, msg->header.frame_id, + ros::Time::now() - ros::Duration(0.2f), source_to_target_tf); + pcl_ros::transformPointCloud(*new_cloud_ptr, *new_cloud_ptr, source_to_target_tf); + } + catch (tf::LookupException& e) + { + ROS_ERROR_STREAM("Transform lookup error, using source frame id '" << msg->header.frame_id + << "'"); + SurfDetect.params_.frame_id = msg->header.frame_id; + } + catch (tf::ExtrapolationException& e) + { + ROS_ERROR_STREAM("Transform lookup error, using source frame id '" << msg->header.frame_id + << "'"); + SurfDetect.params_.frame_id = msg->header.frame_id; + } + } + else + { + ROS_INFO_STREAM("Source cloud is already in frame id '" << msg->header.frame_id + << ", skipping transform"); + } + + // add point cloud + SurfDetect.add_cloud(*new_cloud_ptr); } -int main(int argc,char** argv) +int main(int argc, char** argv) { - ros::init(argc,argv,"surface_detection_node"); - ros::NodeHandle nh; - - // parsing arguments - float acquisition_time = DEFAULT_ACQUISITION_TIME; - std::string frame_id = DEFAULT_FRAME_ID; - - if(pcl::console::find_switch(argc,argv,"-h")) - { - std::cout<( - SEGMENTS_CLOUD_TOPIC,1); - - ros::Publisher markers_publisher = nh.advertise( - MARKER_ARRAY_TOPIC,1); - - - // load paramters - if(!SurfDetect.load_parameters("~/surface_detection/") || !SurfDetect.init()) - { - ROS_ERROR_STREAM("Initialization failed"); - return 0; - } - - // start server - SurfServer.run(); - - // acquire data - ros::Duration total_duration(acquisition_time); - ros::Time start_time = ros::Time::now(); - ros::Subscriber cloud_subs = nh.subscribe(POINT_CLOUD_TOPIC,1,point_cloud_subscriber); - - ROS_INFO_STREAM("Started point cloud acquisition"); - while(ros::ok() && - ((start_time + total_duration) > ros::Time::now())) - { - ros::spinOnce(); - } - cloud_subs.shutdown(); - - // processing data - bool succeeded = SurfDetect.get_acquired_clouds_count() > 0; - if(succeeded) - { - if(SurfDetect.find_surfaces()) - { - ROS_INFO_STREAM(SurfDetect.get_results_summary()); - - } - else - { - ROS_ERROR_STREAM("No surface was found"); - succeeded = false; - } - } - else - { - ROS_ERROR_STREAM("Data acquisition failed"); - succeeded = false; - } - - if(succeeded) - { - ROS_INFO_STREAM("Publishing segments visuals"); - sensor_msgs::PointCloud2 cloud_msg; - visualization_msgs::MarkerArray markers_msg = SurfDetect.get_surface_markers(); - SurfDetect.get_region_colored_cloud(cloud_msg); - - // adding markers to server - for(int i =0;i < markers_msg.markers.size();i++) - { - SurfServer.add_surface(markers_msg.markers[i]); - } - - ros::Duration loop_rate(0.5f); - while(succeeded && ros::ok() ) - { - point_cloud_publisher.publish(cloud_msg); - markers_publisher.publish(markers_msg); - ros::spinOnce(); - - loop_rate.sleep(); - } - - point_cloud_publisher.shutdown(); - markers_publisher.shutdown(); - } - - return 0; - + ros::init(argc, argv, "surface_detection_node"); + ros::NodeHandle nh; + + // parsing arguments + float acquisition_time = DEFAULT_ACQUISITION_TIME; + std::string frame_id = DEFAULT_FRAME_ID; + + if (pcl::console::find_switch(argc, argv, "-h")) + { + std::cout << HELP_TEXT << std::endl; + return 0; + } + + if (pcl::console::find_switch(argc, argv, "-a")) + { + pcl::console::parse(argc, argv, "-a", acquisition_time); + } + + if (pcl::console::find_switch(argc, argv, "-i")) + { + pcl::console::parse(argc, argv, "-i", frame_id); + } + + ROS_INFO_STREAM("Using acquisition time '" << acquisition_time << "'"); + + // publishers + ros::Publisher point_cloud_publisher = + nh.advertise(SEGMENTS_CLOUD_TOPIC, 1); + + ros::Publisher markers_publisher = + nh.advertise(MARKER_ARRAY_TOPIC, 1); + + // load paramters + if (!SurfDetect.load_parameters("~/surface_detection/") || !SurfDetect.init()) + { + ROS_ERROR_STREAM("Initialization failed"); + return 0; + } + + // start server + SurfServer.run(); + + // acquire data + ros::Duration total_duration(acquisition_time); + ros::Time start_time = ros::Time::now(); + ros::Subscriber cloud_subs = nh.subscribe(POINT_CLOUD_TOPIC, 1, point_cloud_subscriber); + + ROS_INFO_STREAM("Started point cloud acquisition"); + while (ros::ok() && ((start_time + total_duration) > ros::Time::now())) + { + ros::spinOnce(); + } + cloud_subs.shutdown(); + + // processing data + bool succeeded = SurfDetect.get_acquired_clouds_count() > 0; + if (succeeded) + { + if (SurfDetect.find_surfaces()) + { + ROS_INFO_STREAM(SurfDetect.get_results_summary()); + } + else + { + ROS_ERROR_STREAM("No surface was found"); + succeeded = false; + } + } + else + { + ROS_ERROR_STREAM("Data acquisition failed"); + succeeded = false; + } + + if (succeeded) + { + ROS_INFO_STREAM("Publishing segments visuals"); + sensor_msgs::PointCloud2 cloud_msg; + visualization_msgs::MarkerArray markers_msg = SurfDetect.get_surface_markers(); + SurfDetect.get_region_colored_cloud(cloud_msg); + + // adding markers to server + for (int i = 0; i < markers_msg.markers.size(); i++) + { + SurfServer.add_surface(markers_msg.markers[i]); + } + + ros::Duration loop_rate(0.5f); + while (succeeded && ros::ok()) + { + point_cloud_publisher.publish(cloud_msg); + markers_publisher.publish(markers_msg); + ros::spinOnce(); + + loop_rate.sleep(); + } + + point_cloud_publisher.shutdown(); + markers_publisher.shutdown(); + } + + return 0; } - - - - diff --git a/godel_surface_detection/src/scan/profilimeter_scan.cpp b/godel_surface_detection/src/scan/profilimeter_scan.cpp index 55f861be..e5f42831 100644 --- a/godel_surface_detection/src/scan/profilimeter_scan.cpp +++ b/godel_surface_detection/src/scan/profilimeter_scan.cpp @@ -9,181 +9,183 @@ using namespace godel_process_path; static const double GROWTH_FACTOR = 1.1; // The scan euclidean distance between subsiquent points -// in the scan trajectory +// in the scan trajectory static const double SCAN_DISCRETIZATION = 0.01; // 1 cm namespace { - /** - * Rotated rectangle structure for use with striping algorithm - * (x,y) is the center (of mass) - * w, h are the edge lengths - * a is the clockwise rotation from upright - */ - struct RotatedRect - { - double x, y, w, h, a; - }; +/** + * Rotated rectangle structure for use with striping algorithm + * (x,y) is the center (of mass) + * w, h are the edge lengths + * a is the clockwise rotation from upright + */ +struct RotatedRect +{ + double x, y, w, h, a; +}; + +RotatedRect simpleBoundingBox(const PolygonBoundary& boundary) +{ + double min_x = boundary[0].x; + double min_y = boundary[0].y; + double max_x = boundary[0].x; + double max_y = boundary[0].y; - RotatedRect simpleBoundingBox(const PolygonBoundary& boundary) + for (std::size_t i = 1; i < boundary.size(); ++i) { - double min_x = boundary[0].x; - double min_y = boundary[0].y; - double max_x = boundary[0].x; - double max_y = boundary[0].y; + const PolygonPt& pt = boundary[i]; + min_x = std::min(pt.x, min_x); + min_y = std::min(pt.y, min_y); + max_x = std::max(pt.x, max_x); + max_y = std::max(pt.y, max_y); + } - for (std::size_t i = 1; i < boundary.size(); ++i) - { - const PolygonPt& pt = boundary[i]; - min_x = std::min(pt.x, min_x); - min_y = std::min(pt.y, min_y); - max_x = std::max(pt.x, max_x); - max_y = std::max(pt.y, max_y); - } + RotatedRect result; + result.a = 0.0; // non-rotated, hence the simple bounding box + result.x = (max_x + min_x) / 2; + result.y = (max_y + min_y) / 2; + result.w = max_x - min_x; + result.h = max_y - min_y; + // grow region a little + result.w *= GROWTH_FACTOR; + result.h *= GROWTH_FACTOR; - RotatedRect result; - result.a = 0.0; // non-rotated, hence the simple bounding box - result.x = (max_x + min_x) / 2; - result.y = (max_y + min_y) / 2; - result.w = max_x - min_x; - result.h = max_y - min_y; + return result; +} - // grow region a little - result.w *= GROWTH_FACTOR; - result.h *= GROWTH_FACTOR; +// Vertically or horizontally? +std::vector sliceBoundingBox(const RotatedRect& bbox, double slice_width, + double overlap) +{ + // we assume slice_width > overlap + int n_slices; + float dx, dy; // unit vector + float x, y; // start position + float w, h; - return result; - } + float adjusted_width = slice_width - overlap; + // walk vertically, horizontal slices + n_slices = static_cast(bbox.h / (adjusted_width)) + 1; - // Vertically or horizontally? - std::vector sliceBoundingBox(const RotatedRect& bbox, double slice_width, double overlap) - { - // we assume slice_width > overlap - int n_slices; - float dx, dy; // unit vector - float x, y; // start position - float w, h; - - float adjusted_width = slice_width - overlap; - // walk vertically, horizontal slices - n_slices = static_cast(bbox.h / (adjusted_width)) + 1; - - dx = -std::sin(bbox.a); - dy = std::cos(bbox.a); - - x = bbox.x - dx * (bbox.h - adjusted_width) / 2.0; - y = bbox.y - dy * (bbox.h - adjusted_width) / 2.0; - - w = bbox.w; - h = adjusted_width; - - std::vector slices; - slices.reserve(n_slices); - - dx *= adjusted_width; - dy *= adjusted_width; - - for (int i = 0; i < n_slices; ++i) - { - RotatedRect temp; - temp.a = bbox.a; - temp.x = x; - temp.y = y; - temp.w = w; - temp.h = h; - slices.push_back(temp); - x += dx; - y += dy; - } + dx = -std::sin(bbox.a); + dy = std::cos(bbox.a); + + x = bbox.x - dx * (bbox.h - adjusted_width) / 2.0; + y = bbox.y - dy * (bbox.h - adjusted_width) / 2.0; + + w = bbox.w; + h = adjusted_width; + + std::vector slices; + slices.reserve(n_slices); - return slices; + dx *= adjusted_width; + dy *= adjusted_width; + + for (int i = 0; i < n_slices; ++i) + { + RotatedRect temp; + temp.a = bbox.a; + temp.x = x; + temp.y = y; + temp.w = w; + temp.h = h; + slices.push_back(temp); + x += dx; + y += dy; } - std::vector interpolateAlongAxis(const RotatedRect& rect, double ds) + return slices; +} + +std::vector interpolateAlongAxis(const RotatedRect& rect, double ds) +{ + std::vector pts; + double dx, dy; // + float x, y; + int n; + n = rect.w / ds; + dx = std::cos(rect.a); + dy = std::sin(rect.a); + x = rect.x - dx * rect.w / 2.0; + y = rect.y - dy * rect.w / 2.0; + + // transform unit vector + dx *= ds; + dy *= ds; + + for (int i = 0; i < n; ++i) { - std::vector pts; - double dx, dy; // - float x, y; - int n; - n = rect.w / ds; - dx = std::cos(rect.a); - dy = std::sin(rect.a); - x = rect.x - dx * rect.w/2.0; - y = rect.y - dy * rect.w/2.0; - - // transform unit vector - dx *= ds; - dy *= ds; - - for (int i = 0; i < n; ++i) - { - PolygonPt pt; - pt.x = x; - pt.y = y; - x += dx; - y += dy; - pts.push_back(pt); - } + PolygonPt pt; + pt.x = x; + pt.y = y; + x += dx; + y += dy; + pts.push_back(pt); + } - return pts; + return pts; +} + +std::vector makeStitch(const PolygonPt& a, const PolygonPt& b, double ds) +{ + double dx = b.x - a.x; + double dy = b.y - a.y; + double s = std::sqrt(dx * dx + dy * dy); + // steps + size_t n = s / ds; + // unit vector + dx /= s; + dy /= s; + + std::vector result; + for (size_t i = 0; i < n; ++i) + { + PolygonPt pt; + pt.x = a.x + i * dx * ds; + pt.y = a.y + i * dy * ds; + result.push_back(pt); } + return result; +} - std::vector makeStitch(const PolygonPt& a, const PolygonPt& b, double ds) +std::vector stitchAndFlatten(const std::vector >& paths, + double ds) +{ + std::vector result; + bool forward = true; + for (std::size_t i = 0; i < paths.size(); ++i) { - double dx = b.x - a.x; - double dy = b.y - a.y; - double s = std::sqrt(dx*dx + dy*dy); - // steps - size_t n = s / ds; - // unit vector - dx /= s; - dy /= s; - - std::vector result; - for (size_t i = 0; i < n; ++i) + // copy current path in given direction + if (forward) { - PolygonPt pt; - pt.x = a.x + i * dx * ds; - pt.y = a.y + i * dy * ds; - result.push_back(pt); + result.insert(result.end(), paths[i].begin(), paths[i].end()); + } + else + { + result.insert(result.end(), paths[i].rbegin(), paths[i].rend()); } - return result; - } - std::vector stitchAndFlatten(const std::vector >& paths, double ds) - { - std::vector result; - bool forward = true; - for (std::size_t i = 0; i < paths.size(); ++i) + forward = !forward; + + // check to see if there is a next strip + if (i + 1 < paths.size()) { - // copy current path in given direction - if (forward) - { - result.insert(result.end(), paths[i].begin(), paths[i].end()); - } - else - { - result.insert(result.end(), paths[i].rbegin(), paths[i].rend()); - } - - forward = !forward; - - // check to see if there is a next strip - if (i+1 < paths.size()) - { - // if so, stitch to the next point - std::vector stitch = makeStitch(*result.rbegin(), (forward ? *(paths[i+1].begin()) : *(paths[i+1].rbegin())), ds); - result.insert(result.end(), stitch.begin(), stitch.end()); - } + // if so, stitch to the next point + std::vector stitch = makeStitch( + *result.rbegin(), (forward ? *(paths[i + 1].begin()) : *(paths[i + 1].rbegin())), ds); + result.insert(result.end(), stitch.begin(), stitch.end()); } - return result; } + return result; +} } // end anon namespace -std::vector +std::vector godel_surface_detection::generateProfilimeterScanPath(const PolygonBoundary& boundary, - const godel_msgs::ScanPlanParameters ¶ms) + const godel_msgs::ScanPlanParameters& params) { std::vector pts; if (boundary.empty()) @@ -194,10 +196,10 @@ godel_surface_detection::generateProfilimeterScanPath(const PolygonBoundary& bou // Step 1 -> compute axis-aligned bounding box; we rely on our reference pose for orientation RotatedRect bbox = simpleBoundingBox(boundary); - + // Step 2 -> slice bounding box into strips std::vector slices = sliceBoundingBox(bbox, params.scan_width, params.overlap); - + // Step 3 -> generate set of dense points along center of each strip std::vector > slice_points; slice_points.reserve(slices.size()); diff --git a/godel_surface_detection/src/scan/robot_scan.cpp b/godel_surface_detection/src/scan/robot_scan.cpp index 8b7be675..4f48a3b0 100644 --- a/godel_surface_detection/src/scan/robot_scan.cpp +++ b/godel_surface_detection/src/scan/robot_scan.cpp @@ -1,17 +1,17 @@ /* - Copyright Apr 14, 2014 Southwest Research Institute + Copyright Apr 14, 2014 Southwest Research Institute - 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 + 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 + 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. + 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. */ #include @@ -26,363 +26,370 @@ #include #include - -static const std::string DEFAULT_MOVEIT_PLANNER = "RRTConnectkConfigDefault"; +static const std::string DEFAULT_MOVEIT_PLANNER = "RRTConnectkConfigDefault"; static bool loadPoseParam(ros::NodeHandle& nh, const std::string& name, geometry_msgs::Pose& pose) { - using namespace godel_param_helpers; - return loadParam(nh, name + "/trans/x", pose.position.x) && - loadParam(nh, name + "/trans/y", pose.position.y) && - loadParam(nh, name + "/trans/z", pose.position.z) && - loadParam(nh, name + "/quat/x", pose.orientation.x) && - loadParam(nh, name + "/quat/y", pose.orientation.y) && - loadParam(nh, name + "/quat/z", pose.orientation.z) && - loadParam(nh, name + "/quat/w", pose.orientation.w); + using namespace godel_param_helpers; + return loadParam(nh, name + "/trans/x", pose.position.x) && + loadParam(nh, name + "/trans/y", pose.position.y) && + loadParam(nh, name + "/trans/z", pose.position.z) && + loadParam(nh, name + "/quat/x", pose.orientation.x) && + loadParam(nh, name + "/quat/y", pose.orientation.y) && + loadParam(nh, name + "/quat/z", pose.orientation.z) && + loadParam(nh, name + "/quat/w", pose.orientation.w); } -namespace godel_surface_detection { -namespace scan { +namespace godel_surface_detection +{ +namespace scan +{ const double RobotScan::PLANNING_TIME = 60.0f; const double RobotScan::WAIT_MSG_DURATION = 5.0f; const double RobotScan::MIN_TRAJECTORY_TIME_STEP = 0.8f; // seconds -const double RobotScan::EEF_STEP = 0.05f; // 5cm -const double RobotScan::MIN_JOINT_VELOCITY = 0.01f ; // rad/sect +const double RobotScan::EEF_STEP = 0.05f; // 5cm +const double RobotScan::MIN_JOINT_VELOCITY = 0.01f; // rad/sect RobotScan::RobotScan() { - params_.group_name = "manipulator_asus"; - params_.home_position = "home"; - params_.world_frame = "world_frame"; - params_.tcp_frame = "kinect2_move_frame"; - tf::poseTFToMsg(tf::Transform::getIdentity(),params_.tcp_to_cam_pose); - tf::poseTFToMsg(tf::Transform::getIdentity(),params_.world_to_obj_pose); - params_.cam_to_obj_zoffset = 0; - params_.cam_to_obj_xoffset = 0; - params_.cam_tilt_angle = -M_PI/4; - params_.sweep_angle_start = 0; - params_.sweep_angle_end = 2*M_PI; - params_.scan_topic = "point_cloud"; - params_.scan_target_frame = "world_frame"; - params_.reachable_scan_points_ratio = 0.5f; - params_.num_scan_points = 20; - params_.stop_on_planning_error = true; + params_.group_name = "manipulator_asus"; + params_.home_position = "home"; + params_.world_frame = "world_frame"; + params_.tcp_frame = "kinect2_move_frame"; + tf::poseTFToMsg(tf::Transform::getIdentity(), params_.tcp_to_cam_pose); + tf::poseTFToMsg(tf::Transform::getIdentity(), params_.world_to_obj_pose); + params_.cam_to_obj_zoffset = 0; + params_.cam_to_obj_xoffset = 0; + params_.cam_tilt_angle = -M_PI / 4; + params_.sweep_angle_start = 0; + params_.sweep_angle_end = 2 * M_PI; + params_.scan_topic = "point_cloud"; + params_.scan_target_frame = "world_frame"; + params_.reachable_scan_points_ratio = 0.5f; + params_.num_scan_points = 20; + params_.stop_on_planning_error = true; } - bool RobotScan::init() { - move_group_ptr_ = MoveGroupPtr(new move_group_interface::MoveGroup(params_.group_name)); - move_group_ptr_->setEndEffectorLink(params_.tcp_frame); - move_group_ptr_->setPoseReferenceFrame(params_.world_frame); - move_group_ptr_->setPlanningTime(PLANNING_TIME); - move_group_ptr_->setPlannerId(DEFAULT_MOVEIT_PLANNER); - tf_listener_ptr_ = TransformListenerPtr(new tf::TransformListener()); - scan_traj_poses_.clear(); - callback_list_.clear(); - return true; + move_group_ptr_ = MoveGroupPtr(new move_group_interface::MoveGroup(params_.group_name)); + move_group_ptr_->setEndEffectorLink(params_.tcp_frame); + move_group_ptr_->setPoseReferenceFrame(params_.world_frame); + move_group_ptr_->setPlanningTime(PLANNING_TIME); + move_group_ptr_->setPlannerId(DEFAULT_MOVEIT_PLANNER); + tf_listener_ptr_ = TransformListenerPtr(new tf::TransformListener()); + scan_traj_poses_.clear(); + callback_list_.clear(); + return true; } bool RobotScan::load_parameters(const std::string& filename) { - using godel_param_helpers::loadParam; - using godel_param_helpers::loadBoolParam; - - if (godel_param_helpers::fromFile(filename, params_)) - { - return true; - } - // otherwise load from parameters - ros::NodeHandle nh ("~/robot_scan"); - return loadParam(nh, "group_name", params_.group_name) && - loadParam(nh, "home_position", params_.home_position) && - loadParam(nh, "world_frame", params_.world_frame) && - loadParam(nh, "tcp_frame", params_.tcp_frame) && - loadPoseParam(nh, "tcp_to_cam_pose", params_.tcp_to_cam_pose)&& - loadPoseParam(nh, "world_to_obj_pose", params_.world_to_obj_pose) && - loadParam(nh, "cam_to_obj_zoffset", params_.cam_to_obj_zoffset) && - loadParam(nh, "cam_to_obj_xoffset", params_.cam_to_obj_xoffset) && - loadParam(nh, "cam_tilt_angle", params_.cam_tilt_angle) && - loadParam(nh, "sweep_angle_start", params_.sweep_angle_start) && - loadParam(nh, "sweep_angle_end", params_.sweep_angle_end) && - loadParam(nh, "scan_topic", params_.scan_topic) && - loadParam(nh, "num_scan_points", params_.num_scan_points) && - loadParam(nh, "reachable_scan_points_ratio", params_.reachable_scan_points_ratio) && - loadParam(nh, "scan_target_frame", params_.scan_target_frame) && - loadBoolParam(nh, "stop_on_planning_error", params_.stop_on_planning_error); + using godel_param_helpers::loadParam; + using godel_param_helpers::loadBoolParam; + + if (godel_param_helpers::fromFile(filename, params_)) + { + return true; + } + // otherwise load from parameters + ros::NodeHandle nh("~/robot_scan"); + return loadParam(nh, "group_name", params_.group_name) && + loadParam(nh, "home_position", params_.home_position) && + loadParam(nh, "world_frame", params_.world_frame) && + loadParam(nh, "tcp_frame", params_.tcp_frame) && + loadPoseParam(nh, "tcp_to_cam_pose", params_.tcp_to_cam_pose) && + loadPoseParam(nh, "world_to_obj_pose", params_.world_to_obj_pose) && + loadParam(nh, "cam_to_obj_zoffset", params_.cam_to_obj_zoffset) && + loadParam(nh, "cam_to_obj_xoffset", params_.cam_to_obj_xoffset) && + loadParam(nh, "cam_tilt_angle", params_.cam_tilt_angle) && + loadParam(nh, "sweep_angle_start", params_.sweep_angle_start) && + loadParam(nh, "sweep_angle_end", params_.sweep_angle_end) && + loadParam(nh, "scan_topic", params_.scan_topic) && + loadParam(nh, "num_scan_points", params_.num_scan_points) && + loadParam(nh, "reachable_scan_points_ratio", params_.reachable_scan_points_ratio) && + loadParam(nh, "scan_target_frame", params_.scan_target_frame) && + loadBoolParam(nh, "stop_on_planning_error", params_.stop_on_planning_error); } void RobotScan::save_parameters(const std::string& filename) { - if (!godel_param_helpers::toFile(filename, params_)) - { - ROS_WARN_STREAM("Unable to save macro scan-parameters to: " << filename); - } + if (!godel_param_helpers::toFile(filename, params_)) + { + ROS_WARN_STREAM("Unable to save macro scan-parameters to: " << filename); + } } -void RobotScan::add_scan_callback(ScanCallback cb) -{ - callback_list_.push_back(cb); -} +void RobotScan::add_scan_callback(ScanCallback cb) { callback_list_.push_back(cb); } -bool RobotScan::generate_scan_display_trajectory(moveit_msgs::DisplayTrajectory &traj_data) +bool RobotScan::generate_scan_display_trajectory(moveit_msgs::DisplayTrajectory& traj_data) { - // create trajectory - std::vector poses; - bool succeeded = true; - moveit_msgs::RobotTrajectory robot_traj; - if(create_scan_trajectory(poses,robot_traj)) - { - traj_data.trajectory.push_back(robot_traj); - } - else - { - succeeded = false; - } - return succeeded; + // create trajectory + std::vector poses; + bool succeeded = true; + moveit_msgs::RobotTrajectory robot_traj; + if (create_scan_trajectory(poses, robot_traj)) + { + traj_data.trajectory.push_back(robot_traj); + } + else + { + succeeded = false; + } + return succeeded; } bool RobotScan::generate_scan_poses(geometry_msgs::PoseArray& poses) { - // create trajectory - bool succeeded = true; - moveit_msgs::RobotTrajectory robot_traj; - succeeded = create_scan_trajectory(poses.poses,robot_traj); - poses.header.frame_id = params_.world_frame; + // create trajectory + bool succeeded = true; + moveit_msgs::RobotTrajectory robot_traj; + succeeded = create_scan_trajectory(poses.poses, robot_traj); + poses.header.frame_id = params_.world_frame; - return succeeded; + return succeeded; } -void RobotScan::get_latest_scan_poses(geometry_msgs::PoseArray &poses) +void RobotScan::get_latest_scan_poses(geometry_msgs::PoseArray& poses) { - poses.poses.insert(poses.poses.begin(),scan_traj_poses_.begin(),scan_traj_poses_.end()); - poses.header.frame_id = params_.world_frame; + poses.poses.insert(poses.poses.begin(), scan_traj_poses_.begin(), scan_traj_poses_.end()); + poses.header.frame_id = params_.world_frame; } void RobotScan::publish_scan_poses(std::string topic) { - ros::NodeHandle nh; - ros::Publisher poses_pub = nh.advertise(topic,1,true); - geometry_msgs::PoseArray poses_msg; - generate_scan_poses(poses_msg); - poses_msg.header.frame_id = params_.world_frame; - poses_pub.publish(poses_msg); - ros::Duration(1.0f).sleep(); + ros::NodeHandle nh; + ros::Publisher poses_pub = nh.advertise(topic, 1, true); + geometry_msgs::PoseArray poses_msg; + generate_scan_poses(poses_msg); + poses_msg.header.frame_id = params_.world_frame; + poses_pub.publish(poses_msg); + ros::Duration(1.0f).sleep(); } bool RobotScan::move_to_pose(geometry_msgs::Pose& target_pose) { - move_group_ptr_->setPoseTarget(target_pose,params_.tcp_frame); - return move_group_ptr_->move(); + move_group_ptr_->setPoseTarget(target_pose, params_.tcp_frame); + return move_group_ptr_->move(); } int RobotScan::scan(bool move_only) { - // cartesian path generation - double eef_step = EEF_STEP;//1*alpha_incr*params_.cam_to_obj_xoffset; - double jump_threshold = 0.0; - moveit::planning_interface::MoveGroup::Plan path_plan; - geometry_msgs::PoseArray cartesian_poses; - path_plan.planning_time_ = PLANNING_TIME; - - // create trajectory - scan_traj_poses_.clear(); - int poses_reached = 0; - moveit_msgs::RobotTrajectory robot_traj; - if(create_scan_trajectory(scan_traj_poses_,robot_traj)) - { - std::vector trajectory_poses; - - // inserting all poses - trajectory_poses.push_back(move_group_ptr_->getCurrentPose(params_.tcp_frame).pose); - trajectory_poses.insert(trajectory_poses.begin()+1,scan_traj_poses_.begin(),scan_traj_poses_.end()); - - for(size_t i = 1;i < trajectory_poses.size(); i++) - { - // reset path plan structure - path_plan.start_state_ = moveit_msgs::RobotState(); - path_plan.trajectory_ = moveit_msgs::RobotTrajectory(); - - // filling cartesian poses for next move - cartesian_poses.poses.clear(); - cartesian_poses.poses.push_back(trajectory_poses[i]); - - move_group_ptr_->setPoseTarget(trajectory_poses[i], params_.tcp_frame); - // creating path plan structure and execute - move_group_ptr_->setStartStateToCurrentState(); - - moveit::planning_interface::MoveGroup::Plan my_plan; - bool success = move_group_ptr_->plan(my_plan); - - if (!success) - { - if(params_.stop_on_planning_error) - { - ROS_ERROR_STREAM("Path Planning to scan position "<execute(my_plan)) - { - poses_reached++; - } - else - { - if(params_.stop_on_planning_error) - { - ROS_ERROR_STREAM("Path Execution to scan position "<(params_.scan_topic,ros::Duration(WAIT_MSG_DURATION)); - pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); - tf::StampedTransform source_to_target_tf; - if(msg) - { - ROS_INFO_STREAM("Cloud message received, converting to target frame '"<< params_.scan_target_frame<<"'"); - - // convert to message to point cloud - pcl::fromROSMsg(*msg,*cloud_ptr); - - // removed nans - std::vector index; - pcl::removeNaNFromPointCloud(*cloud_ptr,*cloud_ptr,index); - - // transforming - if(msg->header.frame_id.compare(params_.scan_target_frame) != 0) - { - try - { - tf_listener_ptr_->lookupTransform(params_.scan_target_frame,msg->header.frame_id,ros::Time(0),source_to_target_tf); - pcl_ros::transformPointCloud(*cloud_ptr,*cloud_ptr,source_to_target_tf); - } - catch(tf::LookupException &e) - { - ROS_ERROR_STREAM("Transform lookup error, using source frame id '"<< msg->header.frame_id<<"'"); - } - catch(tf::ExtrapolationException &e) - { - ROS_ERROR_STREAM("Transform lookup error, using source frame id '"<< msg->header.frame_id<<"'"); - } - } - - for(std::vector::iterator i = callback_list_.begin(); i != callback_list_.end();i++) - { - (*i)(*cloud_ptr); - } - - } - else - { - ROS_ERROR_STREAM("Cloud message not received"); - } - } - else - { - ROS_WARN_STREAM("MOVE_ONLY mode, skipping scan"); - } - - ros::Duration(0.5f).sleep(); - } - } - - return poses_reached; + // cartesian path generation + double eef_step = EEF_STEP; // 1*alpha_incr*params_.cam_to_obj_xoffset; + double jump_threshold = 0.0; + moveit::planning_interface::MoveGroup::Plan path_plan; + geometry_msgs::PoseArray cartesian_poses; + path_plan.planning_time_ = PLANNING_TIME; + + // create trajectory + scan_traj_poses_.clear(); + int poses_reached = 0; + moveit_msgs::RobotTrajectory robot_traj; + if (create_scan_trajectory(scan_traj_poses_, robot_traj)) + { + std::vector trajectory_poses; + + // inserting all poses + trajectory_poses.push_back(move_group_ptr_->getCurrentPose(params_.tcp_frame).pose); + trajectory_poses.insert(trajectory_poses.begin() + 1, scan_traj_poses_.begin(), + scan_traj_poses_.end()); + + for (size_t i = 1; i < trajectory_poses.size(); i++) + { + // reset path plan structure + path_plan.start_state_ = moveit_msgs::RobotState(); + path_plan.trajectory_ = moveit_msgs::RobotTrajectory(); + + // filling cartesian poses for next move + cartesian_poses.poses.clear(); + cartesian_poses.poses.push_back(trajectory_poses[i]); + + move_group_ptr_->setPoseTarget(trajectory_poses[i], params_.tcp_frame); + // creating path plan structure and execute + move_group_ptr_->setStartStateToCurrentState(); + + moveit::planning_interface::MoveGroup::Plan my_plan; + bool success = move_group_ptr_->plan(my_plan); + + if (!success) + { + if (params_.stop_on_planning_error) + { + ROS_ERROR_STREAM("Path Planning to scan position " << i << " failed, quitting scan"); + break; + } + else + { + ROS_WARN_STREAM("Path Planning to scan position " << i << " failed, skipping scan"); + continue; + } + } + + if (move_group_ptr_->execute(my_plan)) + { + poses_reached++; + } + else + { + if (params_.stop_on_planning_error) + { + ROS_ERROR_STREAM("Path Execution to scan position " << i << " failed, quitting scan"); + break; + } + else + { + ROS_WARN_STREAM("Path Execution to scan position " << i << " failed, skipping scan"); + continue; + } + } + + if (!move_only) + { + // get message + ros::Duration(1.0).sleep(); + sensor_msgs::PointCloud2ConstPtr msg = ros::topic::waitForMessage( + params_.scan_topic, ros::Duration(WAIT_MSG_DURATION)); + pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); + tf::StampedTransform source_to_target_tf; + if (msg) + { + ROS_INFO_STREAM("Cloud message received, converting to target frame '" + << params_.scan_target_frame << "'"); + + // convert to message to point cloud + pcl::fromROSMsg(*msg, *cloud_ptr); + + // removed nans + std::vector index; + pcl::removeNaNFromPointCloud(*cloud_ptr, *cloud_ptr, index); + + // transforming + if (msg->header.frame_id.compare(params_.scan_target_frame) != 0) + { + try + { + tf_listener_ptr_->lookupTransform(params_.scan_target_frame, msg->header.frame_id, + ros::Time(0), source_to_target_tf); + pcl_ros::transformPointCloud(*cloud_ptr, *cloud_ptr, source_to_target_tf); + } + catch (tf::LookupException& e) + { + ROS_ERROR_STREAM("Transform lookup error, using source frame id '" + << msg->header.frame_id << "'"); + } + catch (tf::ExtrapolationException& e) + { + ROS_ERROR_STREAM("Transform lookup error, using source frame id '" + << msg->header.frame_id << "'"); + } + } + + for (std::vector::iterator i = callback_list_.begin(); + i != callback_list_.end(); i++) + { + (*i)(*cloud_ptr); + } + } + else + { + ROS_ERROR_STREAM("Cloud message not received"); + } + } + else + { + ROS_WARN_STREAM("MOVE_ONLY mode, skipping scan"); + } + + ros::Duration(0.5f).sleep(); + } + } + + return poses_reached; } -MoveGroupPtr RobotScan::get_move_group() -{ - return move_group_ptr_; -} +MoveGroupPtr RobotScan::get_move_group() { return move_group_ptr_; } -bool RobotScan::create_scan_trajectory(std::vector &scan_poses,moveit_msgs::RobotTrajectory& scan_traj) +bool RobotScan::create_scan_trajectory(std::vector& scan_poses, + moveit_msgs::RobotTrajectory& scan_traj) { - // creating poses - tf::Transform world_to_tcp = tf::Transform::getIdentity(); - tf::Transform world_to_cam = tf::Transform::getIdentity(); - tf::Transform obj_to_cam_pose = tf::Transform::getIdentity(); - tf::Transform tcp_to_cam_tf,world_to_obj_tf; - - // converting pose msg to tf - tf::poseMsgToTF(params_.world_to_obj_pose,world_to_obj_tf); - tf::poseMsgToTF(params_.tcp_to_cam_pose,tcp_to_cam_tf); - - geometry_msgs::Pose pose; - double alpha; - double alpha_incr = (params_.sweep_angle_end - params_.sweep_angle_start)/(params_.num_scan_points -1); - double eef_step = 4*alpha_incr*params_.cam_to_obj_xoffset; - double jump_threshold = 0.0f; - - // relative transforms - tf::Transform xoffset_disp = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(params_.cam_to_obj_xoffset,0,0)); - tf::Transform zoffset_disp = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(0,0,params_.cam_to_obj_zoffset)); - tf::Transform rot_alpha_about_z = tf::Transform::getIdentity(); - tf::Transform rot_tilt_about_y = tf::Transform(tf::Quaternion(tf::Vector3(0,1,0),params_.cam_tilt_angle)); - for(int i = 0; i < params_.num_scan_points;i++) - { - alpha = params_.sweep_angle_start + alpha_incr * i; - rot_alpha_about_z = tf::Transform(tf::Quaternion(tf::Vector3(0,0,1),alpha)); - obj_to_cam_pose = zoffset_disp * rot_alpha_about_z*xoffset_disp*rot_tilt_about_y; - world_to_tcp = world_to_obj_tf * obj_to_cam_pose * tcp_to_cam_tf.inverse(); - tf::poseTFToMsg(world_to_tcp,pose); - scan_poses.push_back(pose); - } - - ROS_INFO_STREAM("Computing cartesian path for a trajectory with "<setEndEffectorLink(params_.tcp_frame); - - ROS_INFO_STREAM("Computing cartesian path for link '"<getEndEffectorLink()<<"'"); - - return true; + // creating poses + tf::Transform world_to_tcp = tf::Transform::getIdentity(); + tf::Transform world_to_cam = tf::Transform::getIdentity(); + tf::Transform obj_to_cam_pose = tf::Transform::getIdentity(); + tf::Transform tcp_to_cam_tf, world_to_obj_tf; + + // converting pose msg to tf + tf::poseMsgToTF(params_.world_to_obj_pose, world_to_obj_tf); + tf::poseMsgToTF(params_.tcp_to_cam_pose, tcp_to_cam_tf); + + geometry_msgs::Pose pose; + double alpha; + double alpha_incr = + (params_.sweep_angle_end - params_.sweep_angle_start) / (params_.num_scan_points - 1); + double eef_step = 4 * alpha_incr * params_.cam_to_obj_xoffset; + double jump_threshold = 0.0f; + + // relative transforms + tf::Transform xoffset_disp = + tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(params_.cam_to_obj_xoffset, 0, 0)); + tf::Transform zoffset_disp = + tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(0, 0, params_.cam_to_obj_zoffset)); + tf::Transform rot_alpha_about_z = tf::Transform::getIdentity(); + tf::Transform rot_tilt_about_y = + tf::Transform(tf::Quaternion(tf::Vector3(0, 1, 0), params_.cam_tilt_angle)); + for (int i = 0; i < params_.num_scan_points; i++) + { + alpha = params_.sweep_angle_start + alpha_incr * i; + rot_alpha_about_z = tf::Transform(tf::Quaternion(tf::Vector3(0, 0, 1), alpha)); + obj_to_cam_pose = zoffset_disp * rot_alpha_about_z * xoffset_disp * rot_tilt_about_y; + world_to_tcp = world_to_obj_tf * obj_to_cam_pose * tcp_to_cam_tf.inverse(); + tf::poseTFToMsg(world_to_tcp, pose); + scan_poses.push_back(pose); + } + + ROS_INFO_STREAM("Computing cartesian path for a trajectory with " + << params_.num_scan_points << " points, eef_step: " << eef_step); + move_group_ptr_->setEndEffectorLink(params_.tcp_frame); + + ROS_INFO_STREAM("Computing cartesian path for link '" << move_group_ptr_->getEndEffectorLink() + << "'"); + + return true; } -void RobotScan::apply_trajectory_parabolic_time_parameterization(robot_trajectory::RobotTrajectory& rt, - moveit_msgs::RobotTrajectory &traj, - unsigned int max_iterations, - double max_time_change_per_it) +void RobotScan::apply_trajectory_parabolic_time_parameterization( + robot_trajectory::RobotTrajectory& rt, moveit_msgs::RobotTrajectory& traj, + unsigned int max_iterations, double max_time_change_per_it) { - // applying filter - trajectory_processing::IterativeParabolicTimeParameterization iter_prmt(max_iterations,max_time_change_per_it); - iter_prmt.computeTimeStamps(rt); - rt.getRobotTrajectoryMsg(traj); - - // removing redundant points - std::vector &points = traj.joint_trajectory.points; - std::vector::iterator iter = points.begin()+1; - double dt; - while(iter != points.end()) - { - trajectory_msgs::JointTrajectoryPoint &p1 = *(iter-1); - trajectory_msgs::JointTrajectoryPoint &p2 = *iter; - - dt = (p2.time_from_start - p1.time_from_start).toSec(); - if(dt < 0.001f) - { - iter = points.erase(iter); - } - else - { - iter++; - } - } + // applying filter + trajectory_processing::IterativeParabolicTimeParameterization iter_prmt(max_iterations, + max_time_change_per_it); + iter_prmt.computeTimeStamps(rt); + rt.getRobotTrajectoryMsg(traj); + + // removing redundant points + std::vector& points = traj.joint_trajectory.points; + std::vector::iterator iter = points.begin() + 1; + double dt; + while (iter != points.end()) + { + trajectory_msgs::JointTrajectoryPoint& p1 = *(iter - 1); + trajectory_msgs::JointTrajectoryPoint& p2 = *iter; + + dt = (p2.time_from_start - p1.time_from_start).toSec(); + if (dt < 0.001f) + { + iter = points.erase(iter); + } + else + { + iter++; + } + } } } /* namespace scan */ diff --git a/godel_surface_detection/src/services/blending_service_path_generation.cpp b/godel_surface_detection/src/services/blending_service_path_generation.cpp index c5f161eb..9ff08672 100644 --- a/godel_surface_detection/src/services/blending_service_path_generation.cpp +++ b/godel_surface_detection/src/services/blending_service_path_generation.cpp @@ -6,7 +6,7 @@ const static std::string BLEND_TRAJECTORY_BAGFILE = "blend_trajectory.bag"; const static std::string BLEND_TRAJECTORY_GROUP_NAME = "manipulator_tcp"; const static std::string BLEND_TRAJECTORY_TOOL_FRAME = "tcp_frame"; const static std::string BLEND_TRAJECTORY_WORLD_FRAME = "world_frame"; -const static double BLEND_TRAJECTORY_ANGLE_DISC = M_PI/10.0; +const static double BLEND_TRAJECTORY_ANGLE_DISC = M_PI / 10.0; // Temporary constants for storing scan path planning parameters // Will be replaced by loadable, savable parameters @@ -52,14 +52,15 @@ filterPolygonBoundaries(const godel_process_path::PolygonBoundaryCollection& bou return filtered_boundaries; } -bool SurfaceBlendingService::requestBlendPath(const godel_process_path::PolygonBoundaryCollection& boundaries, - const geometry_msgs::Pose& boundary_pose, - const godel_msgs::BlendingPlanParameters& params, - visualization_msgs::Marker& path) +bool SurfaceBlendingService::requestBlendPath( + const godel_process_path::PolygonBoundaryCollection& boundaries, + const geometry_msgs::Pose& boundary_pose, const godel_msgs::BlendingPlanParameters& params, + visualization_msgs::Marker& path) { godel_process_path_generation::VisualizeBlendingPlan srv; srv.request.params = params; - godel_process_path::utils::translations::godelToGeometryMsgs(srv.request.surface.boundaries, boundaries); + godel_process_path::utils::translations::godelToGeometryMsgs(srv.request.surface.boundaries, + boundaries); tf::poseTFToMsg(tf::Transform::getIdentity(), srv.request.surface.pose); if (!visualize_process_path_client_.call(srv)) @@ -68,24 +69,26 @@ bool SurfaceBlendingService::requestBlendPath(const godel_process_path::PolygonB } // blend process path calculations suceeded. Save data into results. - path = srv.response.path; + path = srv.response.path; path.ns = PATH_NAMESPACE; path.pose = boundary_pose; return true; } -bool SurfaceBlendingService::requestScanPath(const godel_process_path::PolygonBoundaryCollection& boundaries, - const geometry_msgs::Pose& boundary_pose, - const godel_msgs::ScanPlanParameters& params, - visualization_msgs::Marker& path) +bool SurfaceBlendingService::requestScanPath( + const godel_process_path::PolygonBoundaryCollection& boundaries, + const geometry_msgs::Pose& boundary_pose, const godel_msgs::ScanPlanParameters& params, + visualization_msgs::Marker& path) { using namespace godel_process_path; using godel_process_path::utils::translations::godelToVisualizationMsgs; - if (boundaries.empty()) return false; + if (boundaries.empty()) + return false; - PolygonBoundary scan = godel_surface_detection::generateProfilimeterScanPath(boundaries.front(), params); + PolygonBoundary scan = + godel_surface_detection::generateProfilimeterScanPath(boundaries.front(), params); utils::translations::godelToVisualizationMsgs(path, scan, std_msgs::ColorRGBA()); // Add in an approach and depart vector const geometry_msgs::Point& start_pt = path.points.front(); @@ -115,10 +118,10 @@ bool SurfaceBlendingService::requestScanPath(const godel_process_path::PolygonBo return true; } -ProcessPathResult SurfaceBlendingService::generateProcessPath(const std::string& name, - const pcl::PolygonMesh& mesh, - const godel_msgs::BlendingPlanParameters& blend_params, - const godel_msgs::ScanPlanParameters& scan_params) +ProcessPathResult +SurfaceBlendingService::generateProcessPath(const std::string& name, const pcl::PolygonMesh& mesh, + const godel_msgs::BlendingPlanParameters& blend_params, + const godel_msgs::ScanPlanParameters& scan_params) { using godel_process_path::PolygonBoundaryCollection; using godel_process_path::PolygonBoundary; @@ -132,8 +135,9 @@ ProcessPathResult SurfaceBlendingService::generateProcessPath(const std::string& return result; } - // Read & filter boundaries that are ill-formed or too small - PolygonBoundaryCollection filtered_boundaries = filterPolygonBoundaries(mesh_importer_.getBoundaries(), blend_params); + // Read & filter boundaries that are ill-formed or too small + PolygonBoundaryCollection filtered_boundaries = + filterPolygonBoundaries(mesh_importer_.getBoundaries(), blend_params); // Read pose geometry_msgs::Pose boundary_pose; mesh_importer_.getPose(boundary_pose); @@ -210,8 +214,9 @@ ProcessPathResult SurfaceBlendingService::generateProcessPath(const std::string& return result; } -godel_surface_detection::TrajectoryLibrary SurfaceBlendingService::generateMotionLibrary(const godel_msgs::BlendingPlanParameters& blend_params, - const godel_msgs::ScanPlanParameters& scan_params) +godel_surface_detection::TrajectoryLibrary SurfaceBlendingService::generateMotionLibrary( + const godel_msgs::BlendingPlanParameters& blend_params, + const godel_msgs::ScanPlanParameters& scan_params) { std::vector meshes; surface_server_.get_selected_surfaces(meshes); @@ -231,7 +236,8 @@ godel_surface_detection::TrajectoryLibrary SurfaceBlendingService::generateMotio ProcessPathResult paths = generateProcessPath(names[i], meshes[i], blend_params, scan_params); for (std::size_t j = 0; j < paths.paths.size(); ++j) { - ProcessPlanResult plan = generateProcessPlan(paths.paths[j].first, paths.paths[j].second, blend_params, scan_params); + ProcessPlanResult plan = generateProcessPlan(paths.paths[j].first, paths.paths[j].second, + blend_params, scan_params); for (std::size_t k = 0; k < plan.plans.size(); ++k) { lib.get()[plan.plans[k].first] = plan.plans[k].second; @@ -243,15 +249,17 @@ godel_surface_detection::TrajectoryLibrary SurfaceBlendingService::generateMotio static inline bool isBlendingPath(const std::string& name) { - const static std::string suffix ("_blend"); - if (name.size() < suffix.size()) return false; - return name.find(suffix, name.size()-suffix.length()) != std::string::npos; + const static std::string suffix("_blend"); + if (name.size() < suffix.size()) + return false; + return name.find(suffix, name.size() - suffix.length()) != std::string::npos; } -ProcessPlanResult SurfaceBlendingService::generateProcessPlan(const std::string& name, - const visualization_msgs::Marker& path, - const godel_msgs::BlendingPlanParameters& params, - const godel_msgs::ScanPlanParameters &scan_params) +ProcessPlanResult +SurfaceBlendingService::generateProcessPlan(const std::string& name, + const visualization_msgs::Marker& path, + const godel_msgs::BlendingPlanParameters& params, + const godel_msgs::ScanPlanParameters& scan_params) { ProcessPlanResult result; diff --git a/godel_surface_detection/src/services/surface_blending_service.cpp b/godel_surface_detection/src/services/surface_blending_service.cpp index 7b774179..1ef3db38 100644 --- a/godel_surface_detection/src/services/surface_blending_service.cpp +++ b/godel_surface_detection/src/services/surface_blending_service.cpp @@ -16,7 +16,7 @@ const static std::string TRAJECTORY_PLANNING_SERVICE = "trajectory_planner"; const static std::string SURFACE_DETECTION_SERVICE = "surface_detection"; const static std::string SURFACE_BLENDING_PARAMETERS_SERVICE = "surface_blending_parameters"; const static std::string SELECT_SURFACE_SERVICE = "select_surface"; -const static std::string PROCESS_PATH_SERVICE="process_path"; +const static std::string PROCESS_PATH_SERVICE = "process_path"; const static std::string VISUALIZE_BLENDING_PATH_SERVICE = "visualize_path_generator"; const static std::string RENAME_SURFACE_SERVICE = "rename_surface"; const static std::string PATH_EXECUTION_SERVICE = "path_execution"; @@ -33,7 +33,7 @@ const static std::string TOOL_PATH_PREVIEW_TOPIC = "tool_path_preview"; const static std::string SELECTED_SURFACES_CHANGED_TOPIC = "selected_surfaces_changed"; const static std::string ROBOT_SCAN_PATH_PREVIEW_TOPIC = "robot_scan_path_preview"; const static std::string PUBLISH_REGION_POINT_CLOUD = "publish_region_point_cloud"; -const static std::string REGION_POINT_CLOUD_TOPIC="region_colored_cloud"; +const static std::string REGION_POINT_CLOUD_TOPIC = "region_colored_cloud"; // tool visual properties const static float TOOL_DIA = .050; @@ -51,11 +51,12 @@ const static std::string ROBOT_SCAN_PARAMS_FILE = "godel_robot_scan_parameters.m const static std::string SURFACE_DETECTION_PARAMS_FILE = "godel_surface_detection_parameters.msg"; - SurfaceBlendingService::SurfaceBlendingService() - : mesh_importer_(true) /*True-turn on verbose messages*/ - , publish_region_point_cloud_(false) -{} + : mesh_importer_(true) /*True-turn on verbose messages*/ + , + publish_region_point_cloud_(false) +{ +} bool SurfaceBlendingService::init() { @@ -64,7 +65,7 @@ bool SurfaceBlendingService::init() ros::NodeHandle ph("~"); // loading parameters - ph.getParam(PUBLISH_REGION_POINT_CLOUD,publish_region_point_cloud_); + ph.getParam(PUBLISH_REGION_POINT_CLOUD, publish_region_point_cloud_); // Load the 'prefix' that will be combined with parameters msg base names // to save to disk ph.param("param_cache_prefix", param_cache_prefix_, ""); @@ -92,10 +93,11 @@ bool SurfaceBlendingService::init() default_blending_plan_params_ = blending_plan_params_; default_scan_params_ = scan_plan_params_; - if(surface_detection_.init() && robot_scan_.init() && surface_server_.init()) + if (surface_detection_.init() && robot_scan_.init() && surface_server_.init()) { // adding callbacks - scan::RobotScan::ScanCallback cb = boost::bind(&detection::SurfaceDetection::add_cloud,&surface_detection_,_1); + scan::RobotScan::ScanCallback cb = + boost::bind(&detection::SurfaceDetection::add_cloud, &surface_detection_, _1); robot_scan_.add_scan_callback(cb); ROS_INFO_STREAM("Surface detection service initialization succeeded"); } @@ -105,55 +107,63 @@ bool SurfaceBlendingService::init() } // start server - interactive::InteractiveSurfaceServer::SelectionCallback f = boost::bind( - &SurfaceBlendingService::publish_selected_surfaces_changed,this); + interactive::InteractiveSurfaceServer::SelectionCallback f = + boost::bind(&SurfaceBlendingService::publish_selected_surfaces_changed, this); surface_server_.add_selection_callback(f); // initializing ros interface ros::NodeHandle nh; - + // service clients visualize_process_path_client_ = - nh.serviceClient(VISUALIZE_BLENDING_PATH_SERVICE); + nh.serviceClient( + VISUALIZE_BLENDING_PATH_SERVICE); // Process Execution Parameters - blend_process_client_ = nh.serviceClient(BLEND_PROCESS_EXECUTION_SERVICE); - scan_process_client_ = nh.serviceClient(SCAN_PROCESS_EXECUTION_SERVICE); + blend_process_client_ = + nh.serviceClient(BLEND_PROCESS_EXECUTION_SERVICE); + scan_process_client_ = + nh.serviceClient(SCAN_PROCESS_EXECUTION_SERVICE); - blend_planning_client_ = nh.serviceClient(BLEND_PROCESS_PLANNING_SERVICE); - keyence_planning_client_ = nh.serviceClient(SCAN_PROCESS_PLANNING_SERVICE); + blend_planning_client_ = + nh.serviceClient(BLEND_PROCESS_PLANNING_SERVICE); + keyence_planning_client_ = + nh.serviceClient(SCAN_PROCESS_PLANNING_SERVICE); // service servers - surf_blend_parameters_server_ = nh.advertiseService(SURFACE_BLENDING_PARAMETERS_SERVICE, - &SurfaceBlendingService::surface_blend_parameters_server_callback,this); + surf_blend_parameters_server_ = + nh.advertiseService(SURFACE_BLENDING_PARAMETERS_SERVICE, + &SurfaceBlendingService::surface_blend_parameters_server_callback, this); + + surface_detect_server_ = nh.advertiseService( + SURFACE_DETECTION_SERVICE, &SurfaceBlendingService::surface_detection_server_callback, this); - surface_detect_server_ = nh.advertiseService(SURFACE_DETECTION_SERVICE, - &SurfaceBlendingService::surface_detection_server_callback,this); + select_surface_server_ = nh.advertiseService( + SELECT_SURFACE_SERVICE, &SurfaceBlendingService::select_surface_server_callback, this); - select_surface_server_ = nh.advertiseService(SELECT_SURFACE_SERVICE, - &SurfaceBlendingService::select_surface_server_callback,this); + process_path_server_ = nh.advertiseService( + PROCESS_PATH_SERVICE, &SurfaceBlendingService::process_path_server_callback, this); - process_path_server_ = nh.advertiseService(PROCESS_PATH_SERVICE, - &SurfaceBlendingService::process_path_server_callback,this); + get_motion_plans_server_ = nh.advertiseService( + GET_MOTION_PLANS_SERVICE, &SurfaceBlendingService::getMotionPlansCallback, this); - get_motion_plans_server_ = nh.advertiseService(GET_MOTION_PLANS_SERVICE, - &SurfaceBlendingService::getMotionPlansCallback, this); - - select_motion_plan_server_ = nh.advertiseService(SELECT_MOTION_PLAN_SERVICE, - &SurfaceBlendingService::selectMotionPlanCallback, this); + select_motion_plan_server_ = nh.advertiseService( + SELECT_MOTION_PLAN_SERVICE, &SurfaceBlendingService::selectMotionPlanCallback, this); - load_save_motion_plan_server_ = nh.advertiseService(LOAD_SAVE_MOTION_PLAN_SERVICE, - &SurfaceBlendingService::loadSaveMotionPlanCallback, this); + load_save_motion_plan_server_ = nh.advertiseService( + LOAD_SAVE_MOTION_PLAN_SERVICE, &SurfaceBlendingService::loadSaveMotionPlanCallback, this); rename_suface_server_ = nh.advertiseService(RENAME_SURFACE_SERVICE, - &SurfaceBlendingService::renameSurfaceCallback, this); + &SurfaceBlendingService::renameSurfaceCallback, this); // publishers - selected_surf_changed_pub_ = nh.advertise(SELECTED_SURFACES_CHANGED_TOPIC,1); + selected_surf_changed_pub_ = + nh.advertise(SELECTED_SURFACES_CHANGED_TOPIC, 1); - point_cloud_pub_ = nh.advertise(REGION_POINT_CLOUD_TOPIC,1); + point_cloud_pub_ = nh.advertise(REGION_POINT_CLOUD_TOPIC, 1); - tool_path_markers_pub_ = nh.advertise(TOOL_PATH_PREVIEW_TOPIC,1,true); + tool_path_markers_pub_ = + nh.advertise(TOOL_PATH_PREVIEW_TOPIC, 1, true); return true; } @@ -162,11 +172,10 @@ void SurfaceBlendingService::run() { surface_server_.run(); - ros::Duration loop_duration(1.0f); - while(ros::ok()) + while (ros::ok()) { - if(publish_region_point_cloud_ && !region_cloud_msg_.data.empty()) + if (publish_region_point_cloud_ && !region_cloud_msg_.data.empty()) { point_cloud_pub_.publish(region_cloud_msg_); } @@ -175,7 +184,6 @@ void SurfaceBlendingService::run() } } - // Blending Parameters bool SurfaceBlendingService::load_blend_parameters(const std::string& filename) { @@ -187,7 +195,7 @@ bool SurfaceBlendingService::load_blend_parameters(const std::string& filename) return true; } // otherwise default to the parameter server - ros::NodeHandle nh ("~/blending_plan"); + ros::NodeHandle nh("~/blending_plan"); return loadParam(nh, "tool_radius", blending_plan_params_.tool_radius) && loadParam(nh, "margin", blending_plan_params_.margin) && loadParam(nh, "overlap", blending_plan_params_.overlap) && @@ -229,7 +237,6 @@ bool SurfaceBlendingService::load_scan_parameters(const std::string& filename) loadParam(nh, "max_qa_value", scan_plan_params_.max_qa_value) && loadParam(nh, "approach_distance", scan_plan_params_.approach_distance) && loadParam(nh, "window_width", scan_plan_params_.window_width); - } void SurfaceBlendingService::save_scan_parameters(const std::string& filename) @@ -248,7 +255,7 @@ void SurfaceBlendingService::publish_selected_surfaces_changed() selected_surf_changed_pub_.publish(msg); } -bool SurfaceBlendingService::run_robot_scan(visualization_msgs::MarkerArray &surfaces) +bool SurfaceBlendingService::run_robot_scan(visualization_msgs::MarkerArray& surfaces) { bool succeeded = true; @@ -263,9 +270,9 @@ bool SurfaceBlendingService::run_robot_scan(visualization_msgs::MarkerArray &sur ROS_INFO_STREAM("Starting scan"); int scans_completed = robot_scan_.scan(false); - if(scans_completed > 0) + if (scans_completed > 0) { - ROS_INFO_STREAM("Scan points reached "< meshes; surface_detection_.get_meshes(meshes); - for(std::size_t i =0;i < meshes.size();i++) + for (std::size_t i = 0; i < meshes.size(); i++) { surface_server_.add_surface(meshes[i]); } // copying to surface markers to output argument visualization_msgs::MarkerArray markers_msg = surface_detection_.get_surface_markers(); - surfaces.markers.insert(surfaces.markers.begin(),markers_msg.markers.begin(),markers_msg.markers.end()); + surfaces.markers.insert(surfaces.markers.begin(), markers_msg.markers.begin(), + markers_msg.markers.end()); // saving latest successful results latest_surface_detection_results_.surface_detection = surface_detection_.params_; @@ -318,40 +326,39 @@ bool SurfaceBlendingService::find_surfaces(visualization_msgs::MarkerArray &surf void SurfaceBlendingService::remove_previous_process_plan() { // removing boundary markers - visualization_msgs::MarkerArray &bds = process_path_results_.process_boundaries_; - visualization_msgs::MarkerArray &paths = process_path_results_.process_paths_; - visualization_msgs::MarkerArray &scans = process_path_results_.scan_paths_; + visualization_msgs::MarkerArray& bds = process_path_results_.process_boundaries_; + visualization_msgs::MarkerArray& paths = process_path_results_.process_paths_; + visualization_msgs::MarkerArray& scans = process_path_results_.scan_paths_; - for(std::size_t i = 0; i < bds.markers.size();i++) + for (std::size_t i = 0; i < bds.markers.size(); i++) { - visualization_msgs::Marker &m = bds.markers[i]; + visualization_msgs::Marker& m = bds.markers[i]; m.action = m.DELETE; } - for(std::size_t i = 0; i < paths.markers.size(); i++) + for (std::size_t i = 0; i < paths.markers.size(); i++) { - visualization_msgs::Marker &m = paths.markers[i]; + visualization_msgs::Marker& m = paths.markers[i]; m.action = m.DELETE; } for (std::size_t i = 0; i < scans.markers.size(); ++i) { - visualization_msgs::Marker &m = scans.markers[i]; + visualization_msgs::Marker& m = scans.markers[i]; m.action = m.DELETE; } // publishing markers for deletion visualization_msgs::MarkerArray markers; - markers.markers.insert(markers.markers.end(),bds.markers.begin(),bds.markers.end()); - markers.markers.insert(markers.markers.end(),paths.markers.begin(),paths.markers.end()); - markers.markers.insert(markers.markers.end(),scans.markers.begin(),scans.markers.end()); + markers.markers.insert(markers.markers.end(), bds.markers.begin(), bds.markers.end()); + markers.markers.insert(markers.markers.end(), paths.markers.begin(), paths.markers.end()); + markers.markers.insert(markers.markers.end(), scans.markers.begin(), scans.markers.end()); tool_path_markers_pub_.publish(markers); bds.markers.clear(); paths.markers.clear(); scans.markers.clear(); - } bool SurfaceBlendingService::animate_tool_path() @@ -361,7 +368,7 @@ bool SurfaceBlendingService::animate_tool_path() ROS_INFO_STREAM("Tool animation activated"); - boost::thread (&SurfaceBlendingService::tool_animation_timer_callback, this); + boost::thread(&SurfaceBlendingService::tool_animation_timer_callback, this); return succeeded; } @@ -369,7 +376,7 @@ bool SurfaceBlendingService::animate_tool_path() static bool isBlendPath(const std::string& s) { const static std::string prefix = "_blend"; - return s.find(prefix, s.size()-prefix.size()) != std::string::npos; + return s.find(prefix, s.size() - prefix.size()) != std::string::npos; } void SurfaceBlendingService::tool_animation_timer_callback() @@ -383,38 +390,40 @@ void SurfaceBlendingService::tool_animation_timer_callback() green.r = green.b = 0.f; // marker array for all markers - visualization_msgs::MarkerArray process_markers ; + visualization_msgs::MarkerArray process_markers; // tool marker at arbitrary position - visualization_msgs::MarkerArray tool_markers = create_tool_markers(geometry_msgs::Point(),geometry_msgs::Pose(),"world_frame"); - + visualization_msgs::MarkerArray tool_markers = + create_tool_markers(geometry_msgs::Point(), geometry_msgs::Pose(), "world_frame"); // Hacky thing to get it going for demo // adding markers int num_path_markers = process_path_results_.process_paths_.markers.size(); // Blending Paths - process_markers.markers.insert(process_markers.markers.end(),process_path_results_.process_paths_.markers.begin(), - process_path_results_.process_paths_.markers.end()); + process_markers.markers.insert(process_markers.markers.end(), + process_path_results_.process_paths_.markers.begin(), + process_path_results_.process_paths_.markers.end()); // Surface outlines - process_markers.markers.insert(process_markers.markers.end(),process_path_results_.process_boundaries_.markers.begin(), - process_path_results_.process_boundaries_.markers.end()); + process_markers.markers.insert(process_markers.markers.end(), + process_path_results_.process_boundaries_.markers.begin(), + process_path_results_.process_boundaries_.markers.end()); // The 'tool' - process_markers.markers.insert(process_markers.markers.end(),tool_markers.markers.begin(), - tool_markers.markers.end()); + process_markers.markers.insert(process_markers.markers.end(), tool_markers.markers.begin(), + tool_markers.markers.end()); - ROS_INFO_STREAM(process_path_results_.process_paths_.markers.size() << " path markers, " << - process_path_results_.process_boundaries_.markers.size() << " boundary markers, " << - tool_markers.markers.size() << " tool markers."); + ROS_INFO_STREAM(process_path_results_.process_paths_.markers.size() + << " path markers, " << process_path_results_.process_boundaries_.markers.size() + << " boundary markers, " << tool_markers.markers.size() << " tool markers."); ros::Duration loop_pause(0.02f); - for(int i = 0;i < num_path_markers;i++) + for (int i = 0; i < num_path_markers; i++) { - visualization_msgs::Marker &path_marker = process_markers.markers[i]; + visualization_msgs::Marker& path_marker = process_markers.markers[i]; - for(std::size_t j =0;j < path_marker.points.size();j++) + for (std::size_t j = 0; j < path_marker.points.size(); j++) { - if(stop_tool_animation_) + if (stop_tool_animation_) { ROS_WARN_STREAM("tool path animation completed"); return; @@ -424,12 +433,15 @@ void SurfaceBlendingService::tool_animation_timer_callback() path_marker.colors[j] = green; // updating tool markers - tool_markers = create_tool_markers(path_marker.points[j],path_marker.pose,path_marker.header.frame_id); + tool_markers = + create_tool_markers(path_marker.points[j], path_marker.pose, path_marker.header.frame_id); int start_tool_marker_index = process_markers.markers.size() - tool_markers.markers.size(); - process_markers.markers.erase(boost::next(process_markers.markers.begin(), start_tool_marker_index), process_markers.markers.end()); - process_markers.markers.insert(process_markers.markers.end(), - tool_markers.markers.begin(),tool_markers.markers.end()); + process_markers.markers.erase( + boost::next(process_markers.markers.begin(), start_tool_marker_index), + process_markers.markers.end()); + process_markers.markers.insert(process_markers.markers.end(), tool_markers.markers.begin(), + tool_markers.markers.end()); // publish marker array tool_path_markers_pub_.publish(process_markers); @@ -441,13 +453,14 @@ void SurfaceBlendingService::tool_animation_timer_callback() ROS_INFO_STREAM("tool path animation completed"); } - -visualization_msgs::MarkerArray SurfaceBlendingService::create_tool_markers(const geometry_msgs::Point &pos, const geometry_msgs::Pose &pose,std::string frame_id) +visualization_msgs::MarkerArray +SurfaceBlendingService::create_tool_markers(const geometry_msgs::Point& pos, + const geometry_msgs::Pose& pose, std::string frame_id) { visualization_msgs::MarkerArray tool; tool.markers.resize(2); - visualization_msgs::Marker &disk = tool.markers.at(0); - visualization_msgs::Marker &shaft = tool.markers.at(1); + visualization_msgs::Marker& disk = tool.markers.at(0); + visualization_msgs::Marker& shaft = tool.markers.at(1); std_msgs::ColorRGBA blue; blue.r = 0.; @@ -472,14 +485,14 @@ visualization_msgs::MarkerArray SurfaceBlendingService::create_tool_markers(cons tf::poseMsgToTF(pose, marker_pose); disk.id = 0; - tf::Vector3 marker_pos(pos.x, pos.y, pos.z + .5*TOOL_THK); + tf::Vector3 marker_pos(pos.x, pos.y, pos.z + .5 * TOOL_THK); marker_pos = marker_pose * marker_pos; tf::pointTFToMsg(marker_pos, disk.pose.position); disk.scale.x = disk.scale.y = TOOL_DIA; disk.scale.z = TOOL_THK; shaft.id = 1; - marker_pos = tf::Vector3(pos.x, pos.y, pos.z + TOOL_THK + 0.5*TOOL_SHAFT_LEN); + marker_pos = tf::Vector3(pos.x, pos.y, pos.z + TOOL_THK + 0.5 * TOOL_SHAFT_LEN); marker_pos = marker_pose * marker_pos; tf::pointTFToMsg(marker_pos, shaft.pose.position); shaft.scale.x = shaft.scale.y = TOOL_SHAFT_DIA; @@ -488,20 +501,20 @@ visualization_msgs::MarkerArray SurfaceBlendingService::create_tool_markers(cons return tool; } -bool SurfaceBlendingService::surface_detection_server_callback(godel_msgs::SurfaceDetection::Request &req, - godel_msgs::SurfaceDetection::Response &res) +bool SurfaceBlendingService::surface_detection_server_callback( + godel_msgs::SurfaceDetection::Request& req, godel_msgs::SurfaceDetection::Response& res) { res.surfaces_found = false; res.surfaces = visualization_msgs::MarkerArray(); remove_previous_process_plan(); - switch(req.action) + switch (req.action) { case godel_msgs::SurfaceDetection::Request::PUBLISH_SCAN_PATH: - if(req.use_default_parameters) + if (req.use_default_parameters) { robot_scan_.params_ = default_robot_scan_params__; } @@ -515,7 +528,7 @@ bool SurfaceBlendingService::surface_detection_server_callback(godel_msgs::Surfa case godel_msgs::SurfaceDetection::Request::SCAN_AND_FIND_ONLY: - if(req.use_default_parameters) + if (req.use_default_parameters) { robot_scan_.params_ = default_robot_scan_params__; surface_detection_.params_ = default_surf_detection_params_; @@ -526,13 +539,13 @@ bool SurfaceBlendingService::surface_detection_server_callback(godel_msgs::Surfa surface_detection_.params_ = req.surface_detection; } - res.surfaces_found = run_robot_scan(res.surfaces); + res.surfaces_found = run_robot_scan(res.surfaces); res.surfaces.markers.clear(); break; case godel_msgs::SurfaceDetection::Request::SCAN_FIND_AND_RETURN: - if(req.use_default_parameters) + if (req.use_default_parameters) { robot_scan_.params_ = default_robot_scan_params__; surface_detection_.params_ = default_surf_detection_params_; @@ -543,12 +556,12 @@ bool SurfaceBlendingService::surface_detection_server_callback(godel_msgs::Surfa surface_detection_.params_ = req.surface_detection; } - res.surfaces_found = run_robot_scan(res.surfaces); + res.surfaces_found = run_robot_scan(res.surfaces); break; case godel_msgs::SurfaceDetection::Request::FIND_ONLY: - if(req.use_default_parameters) + if (req.use_default_parameters) { surface_detection_.params_ = default_surf_detection_params_; } @@ -557,13 +570,13 @@ bool SurfaceBlendingService::surface_detection_server_callback(godel_msgs::Surfa surface_detection_.params_ = req.surface_detection; } - res.surfaces_found = find_surfaces(res.surfaces); + res.surfaces_found = find_surfaces(res.surfaces); res.surfaces.markers.clear(); break; case godel_msgs::SurfaceDetection::Request::FIND_AND_RETURN: - if(req.use_default_parameters) + if (req.use_default_parameters) { surface_detection_.params_ = default_surf_detection_params_; } @@ -572,36 +585,36 @@ bool SurfaceBlendingService::surface_detection_server_callback(godel_msgs::Surfa surface_detection_.params_ = req.surface_detection; } - res.surfaces_found = find_surfaces(res.surfaces); + res.surfaces_found = find_surfaces(res.surfaces); break; case godel_msgs::SurfaceDetection::Request::RETURN_LATEST_RESULTS: res = latest_surface_detection_results_; break; - } return true; } -bool SurfaceBlendingService::select_surface_server_callback(godel_msgs::SelectSurface::Request &req, godel_msgs::SelectSurface::Response &) +bool SurfaceBlendingService::select_surface_server_callback(godel_msgs::SelectSurface::Request& req, + godel_msgs::SelectSurface::Response&) { - switch(req.action) + switch (req.action) { case godel_msgs::SelectSurface::Request::SELECT: - for(std::size_t i = 0; req.select_surfaces.size();i++) + for (std::size_t i = 0; req.select_surfaces.size(); i++) { - surface_server_.set_selection_flag(req.select_surfaces[i],true); + surface_server_.set_selection_flag(req.select_surfaces[i], true); } break; case godel_msgs::SelectSurface::Request::DESELECT: - for(std::size_t i = 0; req.select_surfaces.size();i++) + for (std::size_t i = 0; req.select_surfaces.size(); i++) { - surface_server_.set_selection_flag(req.select_surfaces[i],false); + surface_server_.set_selection_flag(req.select_surfaces[i], false); } break; @@ -628,48 +641,52 @@ bool SurfaceBlendingService::select_surface_server_callback(godel_msgs::SelectSu return true; } -bool SurfaceBlendingService::process_path_server_callback(godel_msgs::ProcessPlanning::Request &req, godel_msgs::ProcessPlanning::Response &res) +bool SurfaceBlendingService::process_path_server_callback( + godel_msgs::ProcessPlanning::Request& req, godel_msgs::ProcessPlanning::Response& res) { godel_process_path_generation::VisualizeBlendingPlan process_plan; - process_plan.request.params = req.use_default_parameters ? default_blending_plan_params_: req.params; - godel_msgs::ScanPlanParameters scan_params = req.use_default_parameters ? default_scan_params_ : req.scan_params; - switch(req.action) + process_plan.request.params = + req.use_default_parameters ? default_blending_plan_params_ : req.params; + godel_msgs::ScanPlanParameters scan_params = + req.use_default_parameters ? default_scan_params_ : req.scan_params; + switch (req.action) { - case godel_msgs::ProcessPlanning::Request::GENERATE_MOTION_PLAN: - case godel_msgs::ProcessPlanning::Request::GENERATE_MOTION_PLAN_AND_PREVIEW: - trajectory_library_ = generateMotionLibrary(process_plan.request.params, scan_params); - visualizePaths(); - break; + case godel_msgs::ProcessPlanning::Request::GENERATE_MOTION_PLAN: + case godel_msgs::ProcessPlanning::Request::GENERATE_MOTION_PLAN_AND_PREVIEW: + trajectory_library_ = generateMotionLibrary(process_plan.request.params, scan_params); + visualizePaths(); + break; - case godel_msgs::ProcessPlanning::Request::PREVIEW_TOOL_PATH: + case godel_msgs::ProcessPlanning::Request::PREVIEW_TOOL_PATH: - res.succeeded = animate_tool_path(); - break; + res.succeeded = animate_tool_path(); + break; - case godel_msgs::ProcessPlanning::Request::PREVIEW_MOTION_PLAN: + case godel_msgs::ProcessPlanning::Request::PREVIEW_MOTION_PLAN: - res.succeeded = false; - break; + res.succeeded = false; + break; - case godel_msgs::ProcessPlanning::Request::EXECUTE_MOTION_PLAN: + case godel_msgs::ProcessPlanning::Request::EXECUTE_MOTION_PLAN: - res.succeeded = false; - break; + res.succeeded = false; + break; - default: + default: - ROS_ERROR_STREAM("Unknown action code '"<instantiate(); if (!ptr) diff --git a/industrial_robot_simulator_service/src/simulator_service_node.cpp b/industrial_robot_simulator_service/src/simulator_service_node.cpp index 759dcf5d..20574c69 100644 --- a/industrial_robot_simulator_service/src/simulator_service_node.cpp +++ b/industrial_robot_simulator_service/src/simulator_service_node.cpp @@ -8,7 +8,8 @@ // Constants const static double ACTION_SERVER_WAIT_TIME = 5.0; const static double DEFAULT_SCALE_FACTOR = 0.2; -const static double GOAL_COMPLETION_BUFFER = 0.5; // extra time waited past the end of the trajectory +const static double GOAL_COMPLETION_BUFFER = + 0.5; // extra time waited past the end of the trajectory // Utility Functions static void scaleDurations(trajectory_msgs::JointTrajectory& traj, double scale) @@ -21,95 +22,96 @@ static void scaleDurations(trajectory_msgs::JointTrajectory& traj, double scale) namespace industrial_robot_simulator_service { - class SimulatorService +class SimulatorService +{ +public: + SimulatorService(ros::NodeHandle& nh, const std::string& service_name, + const std::string& action_name) + : ac_(action_name, true), service_name_(service_name), scale_factor_(DEFAULT_SCALE_FACTOR) { - public: - SimulatorService(ros::NodeHandle& nh, const std::string& service_name, const std::string& action_name) - : ac_(action_name, true) - , service_name_(service_name) - , scale_factor_(DEFAULT_SCALE_FACTOR) - { - execute_trajectory_service_ = nh.advertiseService(service_name_, - &SimulatorService::simulateTrajectoryCallback, - this); - - // Establish connection to robot action server - if (!ac_.waitForServer(ros::Duration(ACTION_SERVER_WAIT_TIME))) - { - std::ostringstream ss; - ss << "Could not connect to action: '" << action_name << "'"; - ROS_ERROR_STREAM(ss.str()); - throw std::runtime_error(ss.str()); - } + execute_trajectory_service_ = + nh.advertiseService(service_name_, &SimulatorService::simulateTrajectoryCallback, this); + // Establish connection to robot action server + if (!ac_.waitForServer(ros::Duration(ACTION_SERVER_WAIT_TIME))) + { + std::ostringstream ss; + ss << "Could not connect to action: '" << action_name << "'"; + ROS_ERROR_STREAM(ss.str()); + throw std::runtime_error(ss.str()); } + } - // Makes call to the simulation action server - bool simulateTrajectoryCallback(industrial_robot_simulator_service::SimulateTrajectory::Request& req, - industrial_robot_simulator_service::SimulateTrajectory::Response& res) + // Makes call to the simulation action server + bool + simulateTrajectoryCallback(industrial_robot_simulator_service::SimulateTrajectory::Request& req, + industrial_robot_simulator_service::SimulateTrajectory::Response& res) + { + // If empty trajectory, return true right away + if (req.trajectory.points.empty()) { - // If empty trajectory, return true right away - if (req.trajectory.points.empty()) - { - ROS_WARN("Trajectory simulator recieved empty trajectory"); - return true; - } + ROS_WARN("Trajectory simulator recieved empty trajectory"); + return true; + } - ROS_INFO_STREAM("Handling new simulation service request"); + ROS_INFO_STREAM("Handling new simulation service request"); - // Copy the input header info - control_msgs::FollowJointTrajectoryGoal goal; - goal.trajectory.header = req.trajectory.header; - goal.trajectory.joint_names = req.trajectory.joint_names; + // Copy the input header info + control_msgs::FollowJointTrajectoryGoal goal; + goal.trajectory.header = req.trajectory.header; + goal.trajectory.joint_names = req.trajectory.joint_names; - // Modify the trajectory such that the first point has a duration of zero. - // This forces the industrial_robot_simulator to 'snap' the robot to the - // target position. - goal.trajectory.points = req.trajectory.points; - goal.trajectory.points.front().time_from_start = ros::Duration(0.0); + // Modify the trajectory such that the first point has a duration of zero. + // This forces the industrial_robot_simulator to 'snap' the robot to the + // target position. + goal.trajectory.points = req.trajectory.points; + goal.trajectory.points.front().time_from_start = ros::Duration(0.0); - // Scale the trajectory by the simulator's current time-factor - scaleDurations(goal.trajectory, scale_factor_); + // Scale the trajectory by the simulator's current time-factor + scaleDurations(goal.trajectory, scale_factor_); - ac_.sendGoal(goal); + ac_.sendGoal(goal); - // If the user requested that the server block until the simulation is complete, then - // delay for the time of the trajectory or until the simulator returns. Despite the - // function returning bool, there isn't really a notion of failure here. - if (req.wait_for_execution) + // If the user requested that the server block until the simulation is complete, then + // delay for the time of the trajectory or until the simulator returns. Despite the + // function returning bool, there isn't really a notion of failure here. + if (req.wait_for_execution) + { + ros::Duration wait_time = + goal.trajectory.points.back().time_from_start + ros::Duration(GOAL_COMPLETION_BUFFER); + ROS_DEBUG_STREAM("Waiting for " << wait_time.toSec() << " seconds"); + if (!ac_.waitForResult(wait_time)) { - ros::Duration wait_time = goal.trajectory.points.back().time_from_start + ros::Duration(GOAL_COMPLETION_BUFFER); - ROS_DEBUG_STREAM("Waiting for " << wait_time.toSec() << " seconds"); - if (!ac_.waitForResult(wait_time)) - { - ROS_WARN("Robot Simulator did not successfully complete trajectory"); - } + ROS_WARN("Robot Simulator did not successfully complete trajectory"); } - - return true; } - double scaleFactor() const { return scale_factor_; } + return true; + } - void setScaleFactor(double scale) - { - if (scale > 0.0) scale_factor_ = scale; - else ROS_WARN("Cannot set simulator scale factor to number <= 0.0"); - } + double scaleFactor() const { return scale_factor_; } - private: - actionlib::SimpleActionClient ac_; - ros::ServiceServer execute_trajectory_service_; - std::string service_name_; - double scale_factor_; - }; + void setScaleFactor(double scale) + { + if (scale > 0.0) + scale_factor_ = scale; + else + ROS_WARN("Cannot set simulator scale factor to number <= 0.0"); + } + +private: + actionlib::SimpleActionClient ac_; + ros::ServiceServer execute_trajectory_service_; + std::string service_name_; + double scale_factor_; +}; } int main(int argc, char** argv) { ros::init(argc, argv, "simulation_service_node"); - ros::NodeHandle nh, pnh ("~"); + ros::NodeHandle nh, pnh("~"); // Load parameters std::string service_name; @@ -120,14 +122,14 @@ int main(int argc, char** argv) pnh.param("action_name", action_name, "joint_trajectory_action"); // Initialize the service - industrial_robot_simulator_service::SimulatorService service (nh, service_name, action_name); + industrial_robot_simulator_service::SimulatorService service(nh, service_name, action_name); // Optionally load a new default scale factor if (pnh.getParam("scale_factor", scale_factor)) { service.setScaleFactor(scale_factor); } - + ROS_INFO_STREAM("Simulation service online"); ros::spin(); diff --git a/motoman_sia20d_descartes/include/motoman_sia20d_descartes/motoman_sia20d_robot_model.h b/motoman_sia20d_descartes/include/motoman_sia20d_descartes/motoman_sia20d_robot_model.h index 11e3c36c..c0cf32f1 100644 --- a/motoman_sia20d_descartes/include/motoman_sia20d_descartes/motoman_sia20d_robot_model.h +++ b/motoman_sia20d_descartes/include/motoman_sia20d_descartes/motoman_sia20d_robot_model.h @@ -22,25 +22,23 @@ namespace motoman_sia20d_descartes { - class MotomanSia20dRobotModel: public descartes_moveit::MoveitStateAdapter, - public motoman_sia20d_ikfast_manipulator::ikfast_kinematics_plugin::IKFastKinematicsPlugin +class MotomanSia20dRobotModel + : public descartes_moveit::MoveitStateAdapter, + public motoman_sia20d_ikfast_manipulator::ikfast_kinematics_plugin::IKFastKinematicsPlugin { - public: +public: + MotomanSia20dRobotModel(); - MotomanSia20dRobotModel(); + virtual bool initialize(const std::string& robot_description, const std::string& group_name, + const std::string& world_frame, const std::string& tcp_frame); - virtual bool initialize(const std::string& robot_description, const std::string& group_name, - const std::string& world_frame,const std::string& tcp_frame); - - virtual bool getAllIK(const Eigen::Affine3d &pose, std::vector > &joint_poses) const; - - protected: - - descartes_core::Frame world_to_base_;// world to arm base - descartes_core::Frame tool_to_tip_; // from urdf tool to arm tool + virtual bool getAllIK(const Eigen::Affine3d& pose, + std::vector >& joint_poses) const; +protected: + descartes_core::Frame world_to_base_; // world to arm base + descartes_core::Frame tool_to_tip_; // from urdf tool to arm tool }; - } -#endif //MOTOMAN_SIA20D_ROBOT_MODEL +#endif // MOTOMAN_SIA20D_ROBOT_MODEL diff --git a/motoman_sia20d_descartes/src/motoman_sia20d_robot_model.cpp b/motoman_sia20d_descartes/src/motoman_sia20d_robot_model.cpp index fe3b00ef..3da62413 100644 --- a/motoman_sia20d_descartes/src/motoman_sia20d_robot_model.cpp +++ b/motoman_sia20d_descartes/src/motoman_sia20d_robot_model.cpp @@ -27,102 +27,103 @@ using namespace motoman_sia20d_ikfast_manipulator; namespace motoman_sia20d_descartes { - MotomanSia20dRobotModel::MotomanSia20dRobotModel(): - world_to_base_(Eigen::Affine3d::Identity()), - tool_to_tip_(Eigen::Affine3d::Identity()) - { +MotomanSia20dRobotModel::MotomanSia20dRobotModel() + : world_to_base_(Eigen::Affine3d::Identity()), tool_to_tip_(Eigen::Affine3d::Identity()) +{ +} - } +bool MotomanSia20dRobotModel::initialize(const std::string& robot_description, + const std::string& group_name, + const std::string& world_frame, + const std::string& tcp_frame) +{ + MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame); + ikfast_kinematics_plugin::IKFastKinematicsPlugin::initialize( + robot_description, group_name, MOTOMAN_SIA20D_BASE_LINK, MOTOMAN_SIA20D_TOOL_LINK, 0.001); - bool MotomanSia20dRobotModel::initialize(const std::string& robot_description, const std::string& group_name, - const std::string& world_frame,const std::string& tcp_frame) + // initialize world transformations + if (tcp_frame != getTipFrame()) { - MoveitStateAdapter::initialize(robot_description,group_name,world_frame,tcp_frame); - ikfast_kinematics_plugin::IKFastKinematicsPlugin::initialize(robot_description, group_name, MOTOMAN_SIA20D_BASE_LINK, - MOTOMAN_SIA20D_TOOL_LINK, 0.001); - - // initialize world transformations - if(tcp_frame != getTipFrame()) - { - tool_to_tip_ = descartes_core::Frame(robot_state_->getFrameTransform(tcp_frame).inverse()* - robot_state_->getFrameTransform(getTipFrame())); - } - - if(world_frame != getBaseFrame()) - { - world_to_base_ = descartes_core::Frame(world_to_root_.frame * robot_state_->getFrameTransform(getBaseFrame())); - } - - return true; - + tool_to_tip_ = descartes_core::Frame(robot_state_->getFrameTransform(tcp_frame).inverse() * + robot_state_->getFrameTransform(getTipFrame())); } - bool MotomanSia20dRobotModel::getAllIK(const Eigen::Affine3d &pose, - std::vector< std::vector > &joint_poses) const + if (world_frame != getBaseFrame()) { - bool rtn = false; + world_to_base_ = descartes_core::Frame(world_to_root_.frame * + robot_state_->getFrameTransform(getBaseFrame())); + } - std::vector vfree(free_params_.size()); - KDL::Frame frame; - Eigen::Affine3d tool_pose = world_to_base_.frame_inv* pose* tool_to_tip_.frame; - tf::transformEigenToKDL(tool_pose, frame); + return true; +} - ikfast::IkSolutionList solutions; +bool MotomanSia20dRobotModel::getAllIK(const Eigen::Affine3d& pose, + std::vector >& joint_poses) const +{ + bool rtn = false; - ROS_DEBUG_STREAM("Number of free params: " << vfree.size()); + std::vector vfree(free_params_.size()); + KDL::Frame frame; + Eigen::Affine3d tool_pose = world_to_base_.frame_inv * pose * tool_to_tip_.frame; + tf::transformEigenToKDL(tool_pose, frame); - int numsol = solve(frame, vfree, solutions); + ikfast::IkSolutionList solutions; - ROS_DEBUG_STREAM("Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM("Number of free params: " << vfree.size()); - joint_poses.clear(); + int numsol = solve(frame, vfree, solutions); - if(numsol) - { - for(int s = 0; s < numsol; ++s) - { - std::vector sol; - getSolution(solutions,s,sol); - ROS_DEBUG_STREAM("Original solution size: " << sol.size() - <<", number of joints: " << num_joints_); + ROS_DEBUG_STREAM("Found " << numsol << " solutions from IKFast"); - 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]-JOINT_LIMIT_TOLERANCE)) || - (sol[i] > (joint_max_vector_[i]+JOINT_LIMIT_TOLERANCE)) ) ) - { - // One element of solution is not within limits - obeys_limits = false; - ROS_DEBUG_STREAM("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; - } - } + joint_poses.clear(); - if(obeys_limits) + if (numsol) + { + for (int s = 0; s < numsol; ++s) + { + std::vector sol; + getSolution(solutions, s, sol); + ROS_DEBUG_STREAM("Original solution size: " << sol.size() + << ", number of joints: " << num_joints_); + + 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] - JOINT_LIMIT_TOLERANCE)) || + (sol[i] > (joint_max_vector_[i] + JOINT_LIMIT_TOLERANCE)))) { - joint_poses.push_back(sol); + // One element of solution is not within limits + obeys_limits = false; + ROS_DEBUG_STREAM("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(joint_poses.empty()) - { - ROS_DEBUG_STREAM("GetAllIK has not solutions"); - rtn = false; - } - else - { - rtn = true; + if (obeys_limits) + { + joint_poses.push_back(sol); + } } + } - return rtn; + if (joint_poses.empty()) + { + ROS_DEBUG_STREAM("GetAllIK has not solutions"); + rtn = false; + } + else + { + rtn = true; } + return rtn; +} } - -PLUGINLIB_EXPORT_CLASS(motoman_sia20d_descartes::MotomanSia20dRobotModel,descartes_core::RobotModel) +PLUGINLIB_EXPORT_CLASS(motoman_sia20d_descartes::MotomanSia20dRobotModel, + descartes_core::RobotModel) diff --git a/motoman_sia20d_descartes/src/plugin_init.cpp b/motoman_sia20d_descartes/src/plugin_init.cpp index 3bf1414f..63e0f299 100644 --- a/motoman_sia20d_descartes/src/plugin_init.cpp +++ b/motoman_sia20d_descartes/src/plugin_init.cpp @@ -1,4 +1,5 @@ #include #include -PLUGINLIB_EXPORT_CLASS(motoman_sia20d_descartes::MotomanSia20dRobotModel,descartes_core::RobotModel) +PLUGINLIB_EXPORT_CLASS(motoman_sia20d_descartes::MotomanSia20dRobotModel, + descartes_core::RobotModel) diff --git a/motoman_sia20d_descartes/test/motoman_sia20d_descartes_plugin.cpp b/motoman_sia20d_descartes/test/motoman_sia20d_descartes_plugin.cpp index 0dadf3fb..26c24890 100644 --- a/motoman_sia20d_descartes/test/motoman_sia20d_descartes_plugin.cpp +++ b/motoman_sia20d_descartes/test/motoman_sia20d_descartes_plugin.cpp @@ -11,7 +11,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; Eigen::Affine3d tcp_pose; pluginlib::ClassLoader robot_model_loader("descartes_core", - "descartes_core::RobotModel"); + "descartes_core::RobotModel"); descartes_core::RobotModelPtr robot_model_ptr; TEST(MotomanSia20dRobotModel, createPluginInstance) @@ -21,31 +21,28 @@ TEST(MotomanSia20dRobotModel, createPluginInstance) { robot_model_ptr = robot_model_loader.createInstance(PLUGIN_NAME); succeeded = true; - } catch (pluginlib::PluginlibException& ex) { - ROS_ERROR_STREAM("Failed to create plugin instance of '"<initialize(ROBOT_DESCRIPTION,GROUP_NAME,WORLD_FRAME,TCP_FRAME)); + EXPECT_TRUE(robot_model_ptr->initialize(ROBOT_DESCRIPTION, GROUP_NAME, WORLD_FRAME, TCP_FRAME)); } TEST(MotomanSia20dRobotModel, getFK) { - std::vector joints= {M_PI/4,M_PI/6,M_PI/5,M_PI/8,M_PI/6,0,M_PI/6}; - EXPECT_TRUE(robot_model_ptr->getFK(joints,tcp_pose)); + std::vector joints = {M_PI / 4, M_PI / 6, M_PI / 5, M_PI / 8, M_PI / 6, 0, M_PI / 6}; + EXPECT_TRUE(robot_model_ptr->getFK(joints, tcp_pose)); } TEST(MotomanSia20dRobotModel, getAllIK) { - std::vector< std::vector > solutions; - EXPECT_TRUE(robot_model_ptr->getAllIK(tcp_pose,solutions)); - ROS_INFO_STREAM("Found "< > solutions; + EXPECT_TRUE(robot_model_ptr->getAllIK(tcp_pose, solutions)); + ROS_INFO_STREAM("Found " << solutions.size() << " ik solutions"); } - diff --git a/motoman_sia20d_descartes/test/utest.cpp b/motoman_sia20d_descartes/test/utest.cpp index 2a11062c..d9234ad4 100644 --- a/motoman_sia20d_descartes/test/utest.cpp +++ b/motoman_sia20d_descartes/test/utest.cpp @@ -20,7 +20,7 @@ #include #include -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "utest"); diff --git a/motoman_sia20d_ikfast_manipulator/config/ikfast_sia20d_manipulator.cpp b/motoman_sia20d_ikfast_manipulator/config/ikfast_sia20d_manipulator.cpp index 6f6f6f50..cfcc031b 100644 --- a/motoman_sia20d_ikfast_manipulator/config/ikfast_sia20d_manipulator.cpp +++ b/motoman_sia20d_ikfast_manipulator/config/ikfast_sia20d_manipulator.cpp @@ -5,7 +5,7 @@ /// 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. @@ -16,14 +16,15 @@ /// 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 +/// 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==61); +IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61); #include #include @@ -49,7 +50,16 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #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()); } } +#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 @@ -59,9 +69,9 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #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) +#define IK2PI ((IkReal)6.28318530717959) +#define IKPI ((IkReal)3.14159265358979) +#define IKPI_2 ((IkReal)1.57079632679490) #ifdef _MSC_VER #ifndef isnan @@ -71,25 +81,32 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); // 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); +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 { +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 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); } @@ -111,50 +128,68 @@ inline double IKlog(double f) { return log(f); } 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); + 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); + 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); + 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); + 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); + 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); + 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); } @@ -162,4132 +197,6082 @@ 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 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 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); -} - -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; -} - -/// 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,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63; -x0=IKcos(j[0]); -x1=IKcos(j[3]); -x2=IKcos(j[1]); -x3=IKcos(j[2]); -x4=IKsin(j[1]); -x5=IKsin(j[3]); -x6=IKsin(j[0]); -x7=IKsin(j[2]); -x8=IKsin(j[4]); -x9=IKcos(j[4]); -x10=IKcos(j[6]); -x11=IKsin(j[6]); -x12=IKsin(j[5]); -x13=IKcos(j[5]); -x14=((IkReal(1.00000000000000))*(x7)); -x15=((IkReal(1.00000000000000))*(x1)); -x16=((IkReal(0.180000000000000))*(x9)); -x17=((IkReal(0.420000000000000))*(x2)); -x18=((IkReal(1.00000000000000))*(x9)); -x19=((IkReal(1.00000000000000))*(x2)); -x20=((IkReal(0.180000000000000))*(x1)); -x21=((IkReal(1.00000000000000))*(x13)); -x22=((IkReal(1.00000000000000))*(x8)); -x23=((IkReal(0.180000000000000))*(x5)); -x24=((IkReal(1.00000000000000))*(x5)); -x25=((IkReal(0.420000000000000))*(x1)); -x26=((IkReal(1.00000000000000))*(x12)); -x27=((x0)*(x7)); -x28=((IkReal(-1.00000000000000))*(x13)); -x29=((x3)*(x6)); -x30=((x0)*(x4)); -x31=((x4)*(x8)); -x32=((x4)*(x6)); -x33=((x6)*(x7)); -x34=((x0)*(x3)); -x35=((x3)*(x4)); -x36=((IkReal(-1.00000000000000))*(x12)); -x37=((x14)*(x6)); -x38=((x15)*(x35)); -x39=((x14)*(x4)*(x9)); -x40=((x14)*(x31)); -x41=((((IkReal(-1.00000000000000))*(x37)))+(((x2)*(x34)))); -x42=((((x2)*(x27)))+(x29)); -x43=((((x2)*(x29)))+(x27)); -x44=((((IkReal(-1.00000000000000))*(x34)))+(((x2)*(x33)))); -x45=((((IkReal(-1.00000000000000))*(x38)))+(((x2)*(x5)))); -x46=((((x1)*(x2)))+(((x35)*(x5)))); -x47=((((IkReal(-1.00000000000000))*(x19)*(x34)))+(x37)); -x48=((((IkReal(-1.00000000000000))*(x19)*(x5)))+(x38)); -x49=((((IkReal(-1.00000000000000))*(x0)*(x14)))+(((IkReal(-1.00000000000000))*(x19)*(x29)))); -x50=((x42)*(x8)); -x51=((x1)*(x41)); -x52=((x44)*(x8)); -x53=((x49)*(x5)); -x54=((((x30)*(x5)))+(x51)); -x55=((((x32)*(x5)))+(((x1)*(x43)))); -x56=((((x47)*(x5)))+(((x1)*(x30)))); -x57=((((IkReal(-1.00000000000000))*(x15)*(x43)))+(((IkReal(-1.00000000000000))*(x24)*(x32)))); -x58=((x53)+(((x1)*(x32)))); -x59=((x54)*(x9)); -x60=((x59)+(x50)); -x61=((x52)+(((x55)*(x9)))); -x62=((((x21)*(((((IkReal(-1.00000000000000))*(x45)*(x9)))+(x40)))))+(((IkReal(-1.00000000000000))*(x26)*(x46)))); -x63=((x13)*(x60)); -eerot[0]=((((x10)*(((((x36)*(x56)))+(((x28)*(x60)))))))+(((IkReal(-1.00000000000000))*(x11)*(((((x18)*(x42)))+(((x22)*(((((IkReal(-1.00000000000000))*(x15)*(x41)))+(((IkReal(-1.00000000000000))*(x24)*(x30)))))))))))); -eerot[1]=((((x10)*(((((x42)*(x9)))+(((x8)*(((((IkReal(-1.00000000000000))*(x51)))+(((IkReal(-1.00000000000000))*(x30)*(x5)))))))))))+(((x11)*(((((IkReal(-1.00000000000000))*(x26)*(x56)))+(((IkReal(-1.00000000000000))*(x21)*(x60)))))))); -eerot[2]=((((x12)*(((((IkReal(-1.00000000000000))*(x18)*(x54)))+(((IkReal(-1.00000000000000))*(x22)*(x42)))))))+(((x13)*(x56)))); -eetrans[0]=((((x12)*(((((IkReal(-0.180000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x16)*(x54)))))))+(((IkReal(0.490000000000000))*(x30)))+(((x25)*(x30)))+(((x5)*(((((IkReal(0.420000000000000))*(x33)))+(((IkReal(-1.00000000000000))*(x17)*(x34)))))))+(((x13)*(((((x20)*(x30)))+(((x23)*(x47)))))))); -eerot[3]=((((x11)*(((((IkReal(-1.00000000000000))*(x18)*(x44)))+(((IkReal(-1.00000000000000))*(x22)*(x57)))))))+(((x10)*(((((x36)*(x58)))+(((x28)*(x61)))))))); -eerot[4]=((((x10)*(((((x57)*(x8)))+(((x44)*(x9)))))))+(((x11)*(((((IkReal(-1.00000000000000))*(x26)*(x58)))+(((IkReal(-1.00000000000000))*(x21)*(x61)))))))); -eerot[5]=((((x13)*(x58)))+(((x12)*(((((IkReal(-1.00000000000000))*(x18)*(x55)))+(((IkReal(-1.00000000000000))*(x22)*(x44)))))))); -eetrans[1]=((((x5)*(((((IkReal(-1.00000000000000))*(x17)*(x29)))+(((IkReal(-0.420000000000000))*(x27)))))))+(((x12)*(((((IkReal(-0.180000000000000))*(x52)))+(((IkReal(-1.00000000000000))*(x16)*(x55)))))))+(((IkReal(0.490000000000000))*(x32)))+(((x25)*(x32)))+(((x13)*(((((x20)*(x32)))+(((x23)*(x49)))))))); -eerot[6]=((((x11)*(((((IkReal(-1.00000000000000))*(x22)*(x48)))+(x39)))))+(((x10)*(x62)))); -eerot[7]=((((x11)*(x62)))+(((x10)*(((((IkReal(-1.00000000000000))*(x39)))+(((x48)*(x8)))))))); -eerot[8]=((((x12)*(((((IkReal(-1.00000000000000))*(x18)*(x45)))+(x40)))))+(((x13)*(x46)))); -eetrans[2]=((IkReal(0.410000000000000))+(((x1)*(x17)))+(((x13)*(((((x23)*(x35)))+(((x2)*(x20)))))))+(((x12)*(((((IkReal(0.180000000000000))*(x31)*(x7)))+(((IkReal(-1.00000000000000))*(x16)*(x45)))))))+(((IkReal(0.420000000000000))*(x35)*(x5)))+(((IkReal(0.490000000000000))*(x2)))); -} - -IKFAST_API int GetNumFreeParameters() { return 1; } -IKFAST_API int* GetFreeParameters() { static int freeparams[] = {4}; return freeparams; } -IKFAST_API int GetNumJoints() { return 7; } - -IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } - -IKFAST_API int GetIkType() { return 0x67000001; } - -class IKSolver { -public: -IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j4,cj4,sj4,htj4,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,_ij5[2], _nj5,_ij6[2], _nj6,_ij4[2], _nj4; - -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; j5=numeric_limits::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; j6=numeric_limits::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; _ij4[0] = -1; _ij4[1] = -1; _nj4 = 0; -for(int dummyiter = 0; dummyiter < 1; ++dummyiter) { - solutions.Clear(); -j4=pfree[0]; cj4=cos(pfree[0]); sj4=sin(pfree[0]); -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=((IkReal(-1.00000000000000))*(r00)); -new_r01=r01; -new_r02=((IkReal(-1.00000000000000))*(r02)); -new_px=((((IkReal(-0.180000000000000))*(r02)))+(px)); -new_r10=((IkReal(-1.00000000000000))*(r10)); -new_r11=r11; -new_r12=((IkReal(-1.00000000000000))*(r12)); -new_py=((((IkReal(-0.180000000000000))*(r12)))+(py)); -new_r20=((IkReal(-1.00000000000000))*(r20)); -new_r21=r21; -new_r22=((IkReal(-1.00000000000000))*(r22)); -new_pz=((IkReal(-0.410000000000000))+(pz)+(((IkReal(-0.180000000000000))*(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; -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=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10)))); -rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00)))); -rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00)))); -rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11)))); -rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01)))); -rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01)))); -rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12)))); -rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02)))); -rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02)))); -{ -IkReal j3array[2], cj3array[2], sj3array[2]; -bool j3valid[2]={false}; -_nj3 = 2; -cj3array[0]=((IkReal(-1.01190476190476))+(((IkReal(2.42954324586978))*(pp)))); -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 dummyeval[1]; -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=((IkReal(1.00000000000000))+(((IkReal(2.33333333333333))*(cj3)))+(((IkReal(1.36111111111111))*((cj4)*(cj4))*((sj3)*(sj3))))+(((IkReal(1.36111111111111))*((cj3)*(cj3))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j5array[2], cj5array[2], sj5array[2]; -bool j5valid[2]={false}; -_nj5 = 2; -IkReal x64=((IkReal(-0.420000000000000))+(((IkReal(-0.490000000000000))*(cj3)))); -if( IKabs(x64) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(0.490000000000000))*(cj4)*(sj3))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x65=((IkReal(1.00000000000000))*(IKatan2(x64, ((IkReal(0.490000000000000))*(cj4)*(sj3))))); -if( ((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))) < (IkReal)-0.00001 ) - continue; -if( (((npz)*(((IKabs(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((npz)*(((IKabs(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x66=IKasin(((npz)*(((IKabs(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))); -j5array[0]=((x66)+(((IkReal(-1.00000000000000))*(x65)))); -sj5array[0]=IKsin(j5array[0]); -cj5array[0]=IKcos(j5array[0]); -j5array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x65)))+(((IkReal(-1.00000000000000))*(x66)))); -sj5array[1]=IKsin(j5array[1]); -cj5array[1]=IKcos(j5array[1]); -if( j5array[0] > IKPI ) -{ - j5array[0]-=IK2PI; -} -else if( j5array[0] < -IKPI ) -{ j5array[0]+=IK2PI; -} -j5valid[0] = true; -if( j5array[1] > IKPI ) -{ - j5array[1]-=IK2PI; -} -else if( j5array[1] < -IKPI ) -{ j5array[1]+=IK2PI; -} -j5valid[1] = true; -for(int ij5 = 0; ij5 < 2; ++ij5) +inline float IKsqrt(float f) { -if( !j5valid[ij5] ) -{ - continue; -} -_ij5[0] = ij5; _ij5[1] = -1; -for(int iij5 = ij5+1; iij5 < 2; ++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 dummyeval[1]; -IkReal gconst0; -IkReal x67=((IkReal(100.000000000000))*(sj5)); -gconst0=IKsign(((((x67)*((npx)*(npx))))+(((x67)*((npy)*(npy)))))); -dummyeval[0]=((((sj5)*((npy)*(npy))))+(((sj5)*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst1; -IkReal x68=((IkReal(2100.00000000000))*(sj5)); -gconst1=IKsign(((((x68)*((npx)*(npx))))+(((x68)*((npy)*(npy)))))); -dummyeval[0]=((((sj5)*((npy)*(npy))))+(((sj5)*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[5]; -IkReal x69=((IkReal(1.00000000000000))*(pp)); -IkReal x70=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(((IkReal(-0.490000000000000))*(cj3)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x69)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=x70; -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(-0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x69)))); -evalcond[4]=x70; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst2; -gconst2=IKsign(((((IkReal(-100.000000000000))*((npy)*(npy))))+(((IkReal(-100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((npy)*(npy))))+(((IkReal(-1.00000000000000))*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst3; -IkReal x71=((IkReal(100.000000000000))*(sj4)); -gconst3=IKsign(((((IkReal(-1.00000000000000))*(x71)*((npx)*(npx))))+(((IkReal(-1.00000000000000))*(x71)*((npy)*(npy)))))); -IkReal x72=((IkReal(1.00000000000000))*(sj4)); -dummyeval[0]=((((IkReal(-1.00000000000000))*(x72)*((npx)*(npx))))+(((IkReal(-1.00000000000000))*(x72)*((npy)*(npy))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[5]; -IkReal x73=((IkReal(1.00000000000000))*(pp)); -IkReal x74=x70; -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x73)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=x74; -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(-1.00000000000000))*(x73)))+(((IkReal(-0.840000000000000))*(npz)))); -evalcond[4]=x74; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst4; -gconst4=IKsign(((((IkReal(-100.000000000000))*((npy)*(npy))))+(((IkReal(-100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((npy)*(npy))))+(((IkReal(-1.00000000000000))*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x75=((gconst4)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x75))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x75))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x75)), ((IkReal(-49.0000000000000))*(npx)*(x75))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; + if (f <= 0.0f) + return 0.0f; + return sqrtf(f); } -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x76=IKcos(j6); -IkReal x77=IKsin(j6); -evalcond[0]=((((npx)*(x77)))+(((npy)*(x76)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x77)))+(((npx)*(x76)))+(((IkReal(-0.490000000000000))*(sj3)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -IkReal x234=((IkReal(1.00000000000000))*(pp)); -IkReal x235=x70; -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x234)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=x235; -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(-0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x234)))); -evalcond[4]=x235; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(((((IkReal(100.000000000000))*((npy)*(npy))))+(((IkReal(100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) +inline double IKsqrt(double f) { -continue; - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x236=((gconst5)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x236))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x236))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x236)), ((IkReal(-49.0000000000000))*(npx)*(x236))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; + if (f <= 0.0) + return 0.0; + return sqrt(f); } -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) +inline float IKatan2(float fy, float fx) { -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x237=IKcos(j6); -IkReal x238=IKsin(j6); -evalcond[0]=((((npx)*(x238)))+(((npy)*(x237)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x238)))+(((npx)*(x237)))+(((IkReal(0.490000000000000))*(sj3)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - + 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 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); } -} else -{ -if( 1 ) -{ -continue; - -} else +inline float IKsign(float f) { -} -} -} + if (f > 0) + { + return float(1); + } + else if (f < 0) + { + return float(-1); + } + return 0; } -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x239=(sj4)*(sj4); -IkReal x240=((IkReal(49.0000000000000))*(sj3)*(x239)); -IkReal x241=((IkReal(49.0000000000000))*(cj4)*(sj3)*(sj4)); -if( IKabs(((gconst3)*(((((npy)*(x241)))+(((npx)*(x240))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(npx)*(x241)))+(((npy)*(x240))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst3)*(((((npy)*(x241)))+(((npx)*(x240)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(npx)*(x241)))+(((npy)*(x240))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x242=IKcos(j6); -IkReal x243=IKsin(j6); -IkReal x244=((cj4)*(npy)); -IkReal x245=((IkReal(0.490000000000000))*(sj3)); -IkReal x246=((npy)*(sj4)); -IkReal x247=((IkReal(1.00000000000000))*(x243)); -IkReal x248=((npx)*(x243)); -IkReal x249=((npx)*(x242)); -evalcond[0]=((x248)+(((npy)*(x242)))+(((sj4)*(x245)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x247)))+(((IkReal(-1.00000000000000))*(cj4)*(x245)))+(x249)); -evalcond[2]=((((cj4)*(x248)))+(((x242)*(x244)))+(((sj4)*(x249)))+(((IkReal(-1.00000000000000))*(x246)*(x247)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(npx)*(sj4)*(x247)))+(((cj4)*(x249)))+(((IkReal(-1.00000000000000))*(x245)))+(((IkReal(-1.00000000000000))*(x244)*(x247)))+(((IkReal(-1.00000000000000))*(x242)*(x246)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +inline double IKsign(double f) { -continue; -} -} - -rotationfunction0(solutions); -} -} - + if (f > 0) + { + return 1.0; + } + else if (f < 0) + { + return -1.0; + } + return 0; } +/// 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, x43, x44, x45, x46, x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, + x58, x59, x60, x61, x62, x63; + x0 = IKcos(j[0]); + x1 = IKcos(j[3]); + x2 = IKcos(j[1]); + x3 = IKcos(j[2]); + x4 = IKsin(j[1]); + x5 = IKsin(j[3]); + x6 = IKsin(j[0]); + x7 = IKsin(j[2]); + x8 = IKsin(j[4]); + x9 = IKcos(j[4]); + x10 = IKcos(j[6]); + x11 = IKsin(j[6]); + x12 = IKsin(j[5]); + x13 = IKcos(j[5]); + x14 = ((IkReal(1.00000000000000)) * (x7)); + x15 = ((IkReal(1.00000000000000)) * (x1)); + x16 = ((IkReal(0.180000000000000)) * (x9)); + x17 = ((IkReal(0.420000000000000)) * (x2)); + x18 = ((IkReal(1.00000000000000)) * (x9)); + x19 = ((IkReal(1.00000000000000)) * (x2)); + x20 = ((IkReal(0.180000000000000)) * (x1)); + x21 = ((IkReal(1.00000000000000)) * (x13)); + x22 = ((IkReal(1.00000000000000)) * (x8)); + x23 = ((IkReal(0.180000000000000)) * (x5)); + x24 = ((IkReal(1.00000000000000)) * (x5)); + x25 = ((IkReal(0.420000000000000)) * (x1)); + x26 = ((IkReal(1.00000000000000)) * (x12)); + x27 = ((x0) * (x7)); + x28 = ((IkReal(-1.00000000000000)) * (x13)); + x29 = ((x3) * (x6)); + x30 = ((x0) * (x4)); + x31 = ((x4) * (x8)); + x32 = ((x4) * (x6)); + x33 = ((x6) * (x7)); + x34 = ((x0) * (x3)); + x35 = ((x3) * (x4)); + x36 = ((IkReal(-1.00000000000000)) * (x12)); + x37 = ((x14) * (x6)); + x38 = ((x15) * (x35)); + x39 = ((x14) * (x4) * (x9)); + x40 = ((x14) * (x31)); + x41 = ((((IkReal(-1.00000000000000)) * (x37))) + (((x2) * (x34)))); + x42 = ((((x2) * (x27))) + (x29)); + x43 = ((((x2) * (x29))) + (x27)); + x44 = ((((IkReal(-1.00000000000000)) * (x34))) + (((x2) * (x33)))); + x45 = ((((IkReal(-1.00000000000000)) * (x38))) + (((x2) * (x5)))); + x46 = ((((x1) * (x2))) + (((x35) * (x5)))); + x47 = ((((IkReal(-1.00000000000000)) * (x19) * (x34))) + (x37)); + x48 = ((((IkReal(-1.00000000000000)) * (x19) * (x5))) + (x38)); + x49 = ((((IkReal(-1.00000000000000)) * (x0) * (x14))) + + (((IkReal(-1.00000000000000)) * (x19) * (x29)))); + x50 = ((x42) * (x8)); + x51 = ((x1) * (x41)); + x52 = ((x44) * (x8)); + x53 = ((x49) * (x5)); + x54 = ((((x30) * (x5))) + (x51)); + x55 = ((((x32) * (x5))) + (((x1) * (x43)))); + x56 = ((((x47) * (x5))) + (((x1) * (x30)))); + x57 = ((((IkReal(-1.00000000000000)) * (x15) * (x43))) + + (((IkReal(-1.00000000000000)) * (x24) * (x32)))); + x58 = ((x53) + (((x1) * (x32)))); + x59 = ((x54) * (x9)); + x60 = ((x59) + (x50)); + x61 = ((x52) + (((x55) * (x9)))); + x62 = ((((x21) * (((((IkReal(-1.00000000000000)) * (x45) * (x9))) + (x40))))) + + (((IkReal(-1.00000000000000)) * (x26) * (x46)))); + x63 = ((x13) * (x60)); + eerot[0] = + ((((x10) * (((((x36) * (x56))) + (((x28) * (x60))))))) + + (((IkReal(-1.00000000000000)) * (x11) * + (((((x18) * (x42))) + (((x22) * (((((IkReal(-1.00000000000000)) * (x15) * (x41))) + + (((IkReal(-1.00000000000000)) * (x24) * (x30)))))))))))); + eerot[1] = + ((((x10) * + (((((x42) * (x9))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x51))) + + (((IkReal(-1.00000000000000)) * (x30) * (x5))))))))))) + + (((x11) * (((((IkReal(-1.00000000000000)) * (x26) * (x56))) + + (((IkReal(-1.00000000000000)) * (x21) * (x60)))))))); + eerot[2] = ((((x12) * (((((IkReal(-1.00000000000000)) * (x18) * (x54))) + + (((IkReal(-1.00000000000000)) * (x22) * (x42))))))) + + (((x13) * (x56)))); + eetrans[0] = ((((x12) * (((((IkReal(-0.180000000000000)) * (x50))) + + (((IkReal(-1.00000000000000)) * (x16) * (x54))))))) + + (((IkReal(0.490000000000000)) * (x30))) + (((x25) * (x30))) + + (((x5) * (((((IkReal(0.420000000000000)) * (x33))) + + (((IkReal(-1.00000000000000)) * (x17) * (x34))))))) + + (((x13) * (((((x20) * (x30))) + (((x23) * (x47)))))))); + eerot[3] = ((((x11) * (((((IkReal(-1.00000000000000)) * (x18) * (x44))) + + (((IkReal(-1.00000000000000)) * (x22) * (x57))))))) + + (((x10) * (((((x36) * (x58))) + (((x28) * (x61)))))))); + eerot[4] = ((((x10) * (((((x57) * (x8))) + (((x44) * (x9))))))) + + (((x11) * (((((IkReal(-1.00000000000000)) * (x26) * (x58))) + + (((IkReal(-1.00000000000000)) * (x21) * (x61)))))))); + eerot[5] = ((((x13) * (x58))) + (((x12) * (((((IkReal(-1.00000000000000)) * (x18) * (x55))) + + (((IkReal(-1.00000000000000)) * (x22) * (x44)))))))); + eetrans[1] = ((((x5) * (((((IkReal(-1.00000000000000)) * (x17) * (x29))) + + (((IkReal(-0.420000000000000)) * (x27))))))) + + (((x12) * (((((IkReal(-0.180000000000000)) * (x52))) + + (((IkReal(-1.00000000000000)) * (x16) * (x55))))))) + + (((IkReal(0.490000000000000)) * (x32))) + (((x25) * (x32))) + + (((x13) * (((((x20) * (x32))) + (((x23) * (x49)))))))); + eerot[6] = + ((((x11) * (((((IkReal(-1.00000000000000)) * (x22) * (x48))) + (x39))))) + (((x10) * (x62)))); + eerot[7] = ((((x11) * (x62))) + + (((x10) * (((((IkReal(-1.00000000000000)) * (x39))) + (((x48) * (x8)))))))); + eerot[8] = + ((((x12) * (((((IkReal(-1.00000000000000)) * (x18) * (x45))) + (x40))))) + (((x13) * (x46)))); + eetrans[2] = + ((IkReal(0.410000000000000)) + (((x1) * (x17))) + + (((x13) * (((((x23) * (x35))) + (((x2) * (x20))))))) + + (((x12) * (((((IkReal(0.180000000000000)) * (x31) * (x7))) + + (((IkReal(-1.00000000000000)) * (x16) * (x45))))))) + + (((IkReal(0.420000000000000)) * (x35) * (x5))) + (((IkReal(0.490000000000000)) * (x2)))); } -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x250=((IkReal(49.0000000000000))*(cj4)*(sj3)); -IkReal x251=((IkReal(49.0000000000000))*(sj3)*(sj4)); -if( IKabs(((gconst2)*(((((npy)*(x250)))+(((npx)*(x251))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((npy)*(x251)))+(((IkReal(-1.00000000000000))*(npx)*(x250))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst2)*(((((npy)*(x250)))+(((npx)*(x251)))))), ((gconst2)*(((((npy)*(x251)))+(((IkReal(-1.00000000000000))*(npx)*(x250))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x252=IKcos(j6); -IkReal x253=IKsin(j6); -IkReal x254=((cj4)*(npy)); -IkReal x255=((IkReal(0.490000000000000))*(sj3)); -IkReal x256=((npy)*(sj4)); -IkReal x257=((IkReal(1.00000000000000))*(x253)); -IkReal x258=((npx)*(x253)); -IkReal x259=((npx)*(x252)); -evalcond[0]=((((sj4)*(x255)))+(((npy)*(x252)))+(x258)); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x257)))+(((IkReal(-1.00000000000000))*(cj4)*(x255)))+(x259)); -evalcond[2]=((((sj4)*(x259)))+(((IkReal(-1.00000000000000))*(x256)*(x257)))+(((x252)*(x254)))+(((cj4)*(x258)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x252)*(x256)))+(((IkReal(-1.00000000000000))*(npx)*(sj4)*(x257)))+(((IkReal(-1.00000000000000))*(x254)*(x257)))+(((cj4)*(x259)))+(((IkReal(-1.00000000000000))*(x255)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IKFAST_API int GetNumFreeParameters() { return 1; } +IKFAST_API int* GetFreeParameters() { -continue; -} -} - -rotationfunction0(solutions); -} + static int freeparams[] = {4}; + return freeparams; } +IKFAST_API int GetNumJoints() { return 7; } -} +IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } -} +IKFAST_API int GetIkType() { return 0x67000001; } -} else -{ -IkReal x260=((IkReal(1.00000000000000))*(pp)); -IkReal x261=((IkReal(0.490000000000000))*(cj3)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x260)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=((IkReal(0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(x261)); -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x260)))); -evalcond[4]=((IkReal(-0.420000000000000))+(npz)+(((IkReal(-1.00000000000000))*(x261)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst6; -gconst6=IKsign(((((IkReal(100.000000000000))*((npy)*(npy))))+(((IkReal(100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst7; -IkReal x262=((IkReal(100.000000000000))*(sj4)); -gconst7=IKsign(((((x262)*((npy)*(npy))))+(((x262)*((npx)*(npx)))))); -dummyeval[0]=((((sj4)*((npx)*(npx))))+(((sj4)*((npy)*(npy))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[5]; -IkReal x263=((IkReal(1.00000000000000))*(pp)); -IkReal x264=((IkReal(0.490000000000000))*(cj3)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x263)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=((IkReal(0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(x264)); -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x263)))); -evalcond[4]=((IkReal(-0.420000000000000))+(npz)+(((IkReal(-1.00000000000000))*(x264)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) +class IKSolver { -{ -IkReal dummyeval[1]; -IkReal gconst8; -gconst8=IKsign(((((IkReal(100.000000000000))*((npy)*(npy))))+(((IkReal(100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; +public: + IkReal j0, cj0, sj0, htj0, j1, cj1, sj1, htj1, j2, cj2, sj2, htj2, j3, cj3, sj3, htj3, j5, cj5, + sj5, htj5, j6, cj6, sj6, htj6, j4, cj4, sj4, htj4, 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, _ij5[2], _nj5, _ij6[2], + _nj6, _ij4[2], _nj4; + + 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; + j5 = numeric_limits::quiet_NaN(); + _ij5[0] = -1; + _ij5[1] = -1; + _nj5 = -1; + j6 = numeric_limits::quiet_NaN(); + _ij6[0] = -1; + _ij6[1] = -1; + _nj6 = -1; + _ij4[0] = -1; + _ij4[1] = -1; + _nj4 = 0; + for (int dummyiter = 0; dummyiter < 1; ++dummyiter) + { + solutions.Clear(); + j4 = pfree[0]; + cj4 = cos(pfree[0]); + sj4 = sin(pfree[0]); + 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 = ((IkReal(-1.00000000000000)) * (r00)); + new_r01 = r01; + new_r02 = ((IkReal(-1.00000000000000)) * (r02)); + new_px = ((((IkReal(-0.180000000000000)) * (r02))) + (px)); + new_r10 = ((IkReal(-1.00000000000000)) * (r10)); + new_r11 = r11; + new_r12 = ((IkReal(-1.00000000000000)) * (r12)); + new_py = ((((IkReal(-0.180000000000000)) * (r12))) + (py)); + new_r20 = ((IkReal(-1.00000000000000)) * (r20)); + new_r21 = r21; + new_r22 = ((IkReal(-1.00000000000000)) * (r22)); + new_pz = ((IkReal(-0.410000000000000)) + (pz) + (((IkReal(-0.180000000000000)) * (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; + 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 = ((((IkReal(-1.00000000000000)) * (py) * (r20))) + (((pz) * (r10)))); + rxp0_1 = ((((px) * (r20))) + (((IkReal(-1.00000000000000)) * (pz) * (r00)))); + rxp0_2 = ((((IkReal(-1.00000000000000)) * (px) * (r10))) + (((py) * (r00)))); + rxp1_0 = ((((IkReal(-1.00000000000000)) * (py) * (r21))) + (((pz) * (r11)))); + rxp1_1 = ((((px) * (r21))) + (((IkReal(-1.00000000000000)) * (pz) * (r01)))); + rxp1_2 = ((((IkReal(-1.00000000000000)) * (px) * (r11))) + (((py) * (r01)))); + rxp2_0 = ((((IkReal(-1.00000000000000)) * (py) * (r22))) + (((pz) * (r12)))); + rxp2_1 = ((((px) * (r22))) + (((IkReal(-1.00000000000000)) * (pz) * (r02)))); + rxp2_2 = ((((IkReal(-1.00000000000000)) * (px) * (r12))) + (((py) * (r02)))); + { + IkReal j3array[2], cj3array[2], sj3array[2]; + bool j3valid[2] = {false}; + _nj3 = 2; + cj3array[0] = ((IkReal(-1.01190476190476)) + (((IkReal(2.42954324586978)) * (pp)))); + 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 dummyeval[1]; + dummyeval[0] = (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = + ((IkReal(1.00000000000000)) + (((IkReal(2.33333333333333)) * (cj3))) + + (((IkReal(1.36111111111111)) * ((cj4) * (cj4)) * ((sj3) * (sj3)))) + + (((IkReal(1.36111111111111)) * ((cj3) * (cj3))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j5array[2], cj5array[2], sj5array[2]; + bool j5valid[2] = {false}; + _nj5 = 2; + IkReal x64 = ((IkReal(-0.420000000000000)) + + (((IkReal(-0.490000000000000)) * (cj3)))); + if (IKabs(x64) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(0.490000000000000)) * (cj4) * (sj3))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x65 = + ((IkReal(1.00000000000000)) * + (IKatan2(x64, ((IkReal(0.490000000000000)) * (cj4) * (sj3))))); + if (((((x64) * (x64)) + (((IkReal(0.240100000000000)) * ((cj4) * (cj4)) * + ((sj3) * (sj3)))))) < (IkReal)-0.00001) + continue; + if ((((npz) * + (((IKabs(IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3)))))))) != 0) + ? ((IkReal)1 / + (IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3))))))))) + : (IkReal)1.0e30)))) < -1 - IKFAST_SINCOS_THRESH || + (((npz) * + (((IKabs(IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3)))))))) != 0) + ? ((IkReal)1 / + (IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3))))))))) + : (IkReal)1.0e30)))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x66 = IKasin( + ((npz) * + (((IKabs(IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3)))))))) != 0) + ? ((IkReal)1 / + (IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3))))))))) + : (IkReal)1.0e30)))); + j5array[0] = ((x66) + (((IkReal(-1.00000000000000)) * (x65)))); + sj5array[0] = IKsin(j5array[0]); + cj5array[0] = IKcos(j5array[0]); + j5array[1] = + ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x65))) + + (((IkReal(-1.00000000000000)) * (x66)))); + sj5array[1] = IKsin(j5array[1]); + cj5array[1] = IKcos(j5array[1]); + if (j5array[0] > IKPI) + { + j5array[0] -= IK2PI; + } + else if (j5array[0] < -IKPI) + { + j5array[0] += IK2PI; + } + j5valid[0] = true; + if (j5array[1] > IKPI) + { + j5array[1] -= IK2PI; + } + else if (j5array[1] < -IKPI) + { + j5array[1] += IK2PI; + } + j5valid[1] = true; + for (int ij5 = 0; ij5 < 2; ++ij5) + { + if (!j5valid[ij5]) + { + continue; + } + _ij5[0] = ij5; + _ij5[1] = -1; + for (int iij5 = ij5 + 1; iij5 < 2; ++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 dummyeval[1]; + IkReal gconst0; + IkReal x67 = ((IkReal(100.000000000000)) * (sj5)); + gconst0 = + IKsign(((((x67) * ((npx) * (npx)))) + (((x67) * ((npy) * (npy)))))); + dummyeval[0] = + ((((sj5) * ((npy) * (npy)))) + (((sj5) * ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst1; + IkReal x68 = ((IkReal(2100.00000000000)) * (sj5)); + gconst1 = IKsign( + ((((x68) * ((npx) * (npx)))) + (((x68) * ((npy) * (npy)))))); + dummyeval[0] = + ((((sj5) * ((npy) * (npy)))) + (((sj5) * ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[5]; + IkReal x69 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x70 = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + + (((IkReal(-0.490000000000000)) * (cj3)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j5)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x69))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = x70; + evalcond[3] = ((IkReal(0.0637000000000000)) + + (((IkReal(-0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x69)))); + evalcond[4] = x70; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst2; + gconst2 = IKsign( + ((((IkReal(-100.000000000000)) * ((npy) * (npy)))) + + (((IkReal(-100.000000000000)) * ((npx) * (npx)))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * ((npy) * (npy)))) + + (((IkReal(-1.00000000000000)) * ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst3; + IkReal x71 = ((IkReal(100.000000000000)) * (sj4)); + gconst3 = IKsign(((((IkReal(-1.00000000000000)) * + (x71) * ((npx) * (npx)))) + + (((IkReal(-1.00000000000000)) * + (x71) * ((npy) * (npy)))))); + IkReal x72 = ((IkReal(1.00000000000000)) * (sj4)); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * (x72) * + ((npx) * (npx)))) + + (((IkReal(-1.00000000000000)) * (x72) * + ((npy) * (npy))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[5]; + IkReal x73 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x74 = x70; + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x73))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = x74; + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-1.00000000000000)) * (x73))) + + (((IkReal(-0.840000000000000)) * (npz)))); + evalcond[4] = x74; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst4; + gconst4 = + IKsign(((((IkReal(-100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-1.00000000000000)) * + ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x75 = ((gconst4) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x75))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x75))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((IkReal(49.0000000000000)) * + (npy) * (x75)), + ((IkReal(-49.0000000000000)) * + (npx) * (x75))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x76 = IKcos(j6); + IkReal x77 = IKsin(j6); + evalcond[0] = ((((npx) * (x77))) + + (((npy) * (x76)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * + (npy) * (x77))) + + (((npx) * (x76))) + + (((IkReal(-0.490000000000000)) * + (sj3)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + IkReal x234 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x235 = x70; + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod( + ((IkReal(1.11022302462516e-16)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x234))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = x235; + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x234)))); + evalcond[4] = x235; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst5; + gconst5 = + IKsign(((((IkReal(100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = + (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], + sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x236 = ((gconst5) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x236))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x236))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((IkReal(49.0000000000000)) * + (npy) * (x236)), + ((IkReal(-49.0000000000000)) * + (npx) * (x236))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; + ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x237 = IKcos(j6); + IkReal x238 = IKsin(j6); + evalcond[0] = ((((npx) * (x238))) + + (((npy) * (x237)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * + (npy) * (x238))) + + (((npx) * (x237))) + + (((IkReal(0.490000000000000)) * + (sj3)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x239 = (sj4) * (sj4); + IkReal x240 = + ((IkReal(49.0000000000000)) * (sj3) * (x239)); + IkReal x241 = ((IkReal(49.0000000000000)) * (cj4) * + (sj3) * (sj4)); + if (IKabs(((gconst3) * (((((npy) * (x241))) + + (((npx) * (x240))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst3) * + (((((IkReal(-1.00000000000000)) * (npx) * + (x241))) + + (((npy) * (x240))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst3) * + (((((npy) * (x241))) + (((npx) * (x240)))))), + ((gconst3) * (((((IkReal(-1.00000000000000)) * + (npx) * (x241))) + + (((npy) * (x240))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x242 = IKcos(j6); + IkReal x243 = IKsin(j6); + IkReal x244 = ((cj4) * (npy)); + IkReal x245 = + ((IkReal(0.490000000000000)) * (sj3)); + IkReal x246 = ((npy) * (sj4)); + IkReal x247 = + ((IkReal(1.00000000000000)) * (x243)); + IkReal x248 = ((npx) * (x243)); + IkReal x249 = ((npx) * (x242)); + evalcond[0] = ((x248) + (((npy) * (x242))) + + (((sj4) * (x245)))); + evalcond[1] = ((((IkReal(-1.00000000000000)) * + (npy) * (x247))) + + (((IkReal(-1.00000000000000)) * + (cj4) * (x245))) + + (x249)); + evalcond[2] = + ((((cj4) * (x248))) + (((x242) * (x244))) + + (((sj4) * (x249))) + + (((IkReal(-1.00000000000000)) * (x246) * + (x247)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (npx) * + (sj4) * (x247))) + + (((cj4) * (x249))) + + (((IkReal(-1.00000000000000)) * (x245))) + + (((IkReal(-1.00000000000000)) * (x244) * + (x247))) + + (((IkReal(-1.00000000000000)) * (x242) * + (x246)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x250 = + ((IkReal(49.0000000000000)) * (cj4) * (sj3)); + IkReal x251 = + ((IkReal(49.0000000000000)) * (sj3) * (sj4)); + if (IKabs(((gconst2) * (((((npy) * (x250))) + + (((npx) * (x251))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst2) * (((((npy) * (x251))) + + (((IkReal(-1.00000000000000)) * + (npx) * (x250))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst2) * + (((((npy) * (x250))) + (((npx) * (x251)))))), + ((gconst2) * (((((npy) * (x251))) + + (((IkReal(-1.00000000000000)) * + (npx) * (x250))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x252 = IKcos(j6); + IkReal x253 = IKsin(j6); + IkReal x254 = ((cj4) * (npy)); + IkReal x255 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x256 = ((npy) * (sj4)); + IkReal x257 = ((IkReal(1.00000000000000)) * (x253)); + IkReal x258 = ((npx) * (x253)); + IkReal x259 = ((npx) * (x252)); + evalcond[0] = ((((sj4) * (x255))) + + (((npy) * (x252))) + (x258)); + evalcond[1] = ((((IkReal(-1.00000000000000)) * + (npy) * (x257))) + + (((IkReal(-1.00000000000000)) * + (cj4) * (x255))) + + (x259)); + evalcond[2] = + ((((sj4) * (x259))) + + (((IkReal(-1.00000000000000)) * (x256) * + (x257))) + + (((x252) * (x254))) + (((cj4) * (x258)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x252) * + (x256))) + + (((IkReal(-1.00000000000000)) * (npx) * (sj4) * + (x257))) + + (((IkReal(-1.00000000000000)) * (x254) * + (x257))) + + (((cj4) * (x259))) + + (((IkReal(-1.00000000000000)) * (x255)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + IkReal x260 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x261 = ((IkReal(0.490000000000000)) * (cj3)); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j5)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x260))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = + ((IkReal(0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + (x261)); + evalcond[3] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x260)))); + evalcond[4] = ((IkReal(-0.420000000000000)) + (npz) + + (((IkReal(-1.00000000000000)) * (x261)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst6; + gconst6 = IKsign( + ((((IkReal(100.000000000000)) * ((npy) * (npy)))) + + (((IkReal(100.000000000000)) * ((npx) * (npx)))))); + dummyeval[0] = (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst7; + IkReal x262 = ((IkReal(100.000000000000)) * (sj4)); + gconst7 = IKsign(((((x262) * ((npy) * (npy)))) + + (((x262) * ((npx) * (npx)))))); + dummyeval[0] = ((((sj4) * ((npx) * (npx)))) + + (((sj4) * ((npy) * (npy))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[5]; + IkReal x263 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x264 = + ((IkReal(0.490000000000000)) * (cj3)); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x263))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = + ((IkReal(0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + + (x264)); + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x263)))); + evalcond[4] = + ((IkReal(-0.420000000000000)) + (npz) + + (((IkReal(-1.00000000000000)) * (x264)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst8; + gconst8 = + IKsign(((((IkReal(100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = + (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], + sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x265 = ((gconst8) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x265))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x265))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((IkReal(49.0000000000000)) * + (npy) * (x265)), + ((IkReal(-49.0000000000000)) * + (npx) * (x265))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; + ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x266 = IKcos(j6); + IkReal x267 = IKsin(j6); + evalcond[0] = ((((npx) * (x267))) + + (((npy) * (x266)))); + evalcond[1] = + ((((npx) * (x266))) + + (((IkReal(-1.00000000000000)) * + (npy) * (x267))) + + (((IkReal(0.490000000000000)) * + (sj3)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + IkReal x268 = + ((IkReal(1.00000000000000)) * (pp)); + IkReal x269 = + ((IkReal(0.490000000000000)) * (cj3)); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x268))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = + ((IkReal(0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + + (x269)); + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x268)))); + evalcond[4] = + ((IkReal(-0.420000000000000)) + (npz) + + (((IkReal(-1.00000000000000)) * (x269)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst9; + gconst9 = + IKsign(((((IkReal(-100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-1.00000000000000)) * + ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < + 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], + sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x270 = ((gconst9) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x270))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x270))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((IkReal(49.0000000000000)) * + (npy) * (x270)), + ((IkReal(-49.0000000000000)) * + (npx) * (x270))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; + ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x271 = IKcos(j6); + IkReal x272 = IKsin(j6); + evalcond[0] = ((((npy) * (x271))) + + (((npx) * (x272)))); + evalcond[1] = + ((((IkReal( + -0.490000000000000)) * + (sj3))) + + (((IkReal(-1.00000000000000)) * + (npy) * (x272))) + + (((npx) * (x271)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x273 = (sj4) * (sj4); + IkReal x274 = ((cj4) * (sj4)); + IkReal x275 = + ((IkReal(49.0000000000000)) * (npx) * (sj3)); + IkReal x276 = + ((IkReal(49.0000000000000)) * (npy) * (sj3)); + if (IKabs(((gconst7) * + (((((IkReal(-1.00000000000000)) * + (x273) * (x275))) + + (((x274) * (x276))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst7) * + (((((IkReal(-1.00000000000000)) * + (x273) * (x276))) + + (((IkReal(-1.00000000000000)) * + (x274) * (x275))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst7) * (((((IkReal(-1.00000000000000)) * + (x273) * (x275))) + + (((x274) * (x276)))))), + ((gconst7) * (((((IkReal(-1.00000000000000)) * + (x273) * (x276))) + + (((IkReal(-1.00000000000000)) * + (x274) * (x275))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x277 = IKcos(j6); + IkReal x278 = IKsin(j6); + IkReal x279 = + ((IkReal(1.00000000000000)) * (sj4)); + IkReal x280 = + ((IkReal(0.490000000000000)) * (sj3)); + IkReal x281 = ((npy) * (x277)); + IkReal x282 = ((npx) * (x277)); + IkReal x283 = ((npx) * (x278)); + IkReal x284 = ((npy) * (x278)); + evalcond[0] = + ((x283) + (x281) + (((sj4) * (x280)))); + evalcond[1] = + ((x282) + (((IkReal(-1.00000000000000)) * + (x284))) + + (((cj4) * (x280)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x279) * + (x282))) + + (((cj4) * (x283))) + (((cj4) * (x281))) + + (((sj4) * (x284)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * + (cj4) * (x282))) + + (((IkReal(-1.00000000000000)) * + (x280))) + + (((IkReal(-1.00000000000000)) * + (x279) * (x281))) + + (((IkReal(-1.00000000000000)) * + (x279) * (x283))) + + (((cj4) * (x284)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x285 = + ((IkReal(49.0000000000000)) * (cj4) * (sj3)); + IkReal x286 = + ((IkReal(49.0000000000000)) * (sj3) * (sj4)); + if (IKabs( + ((gconst6) * (((((IkReal(-1.00000000000000)) * + (npx) * (x286))) + + (((npy) * (x285))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst6) * + (((((IkReal(-1.00000000000000)) * (npy) * + (x286))) + + (((IkReal(-1.00000000000000)) * (npx) * + (x285))))))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst6) * (((((IkReal(-1.00000000000000)) * + (npx) * (x286))) + + (((npy) * (x285)))))), + ((gconst6) * (((((IkReal(-1.00000000000000)) * + (npy) * (x286))) + + (((IkReal(-1.00000000000000)) * + (npx) * (x285))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x287 = IKcos(j6); + IkReal x288 = IKsin(j6); + IkReal x289 = + ((IkReal(1.00000000000000)) * (sj4)); + IkReal x290 = + ((IkReal(0.490000000000000)) * (sj3)); + IkReal x291 = ((npy) * (x287)); + IkReal x292 = ((npx) * (x287)); + IkReal x293 = ((npx) * (x288)); + IkReal x294 = ((npy) * (x288)); + evalcond[0] = + ((((sj4) * (x290))) + (x291) + (x293)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x294))) + + (((cj4) * (x290))) + (x292)); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x289) * + (x292))) + + (((cj4) * (x291))) + (((cj4) * (x293))) + + (((sj4) * (x294)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (cj4) * + (x292))) + + (((IkReal(-1.00000000000000)) * (x289) * + (x293))) + + (((IkReal(-1.00000000000000)) * (x289) * + (x291))) + + (((IkReal(-1.00000000000000)) * (x290))) + + (((cj4) * (x294)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x295 = ((IkReal(2500.00000000000)) * (pp)); + IkReal x296 = ((IkReal(2100.00000000000)) * (cj5) * (npz)); + IkReal x297 = + ((IkReal(1029.00000000000)) * (sj3) * (sj4) * (sj5)); + if (IKabs(((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x295))) + + (((IkReal(-1.00000000000000)) * (npy) * (x296))) + + (((IkReal(-1.00000000000000)) * (npx) * (x297))) + + (((IkReal(159.250000000000)) * (npy))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x297))) + + (((npx) * (x295))) + (((npx) * (x296))) + + (((IkReal(-159.250000000000)) * (npx))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x295))) + + (((IkReal(-1.00000000000000)) * (npy) * (x296))) + + (((IkReal(-1.00000000000000)) * (npx) * (x297))) + + (((IkReal(159.250000000000)) * (npy)))))), + ((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x297))) + + (((npx) * (x295))) + (((npx) * (x296))) + + (((IkReal(-159.250000000000)) * (npx))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[6]; + IkReal x298 = IKcos(j6); + IkReal x299 = IKsin(j6); + IkReal x300 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x301 = ((cj5) * (npz)); + IkReal x302 = ((cj4) * (cj5)); + IkReal x303 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x304 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x305 = ((IkReal(0.840000000000000)) * (sj5)); + IkReal x306 = ((npz) * (sj5)); + IkReal x307 = ((npy) * (x298)); + IkReal x308 = ((npx) * (x298)); + IkReal x309 = ((npy) * (x299)); + IkReal x310 = ((npx) * (x299)); + evalcond[0] = ((x307) + (x310) + (((sj4) * (x303)))); + evalcond[1] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-1.00000000000000)) * (x305) * (x309))) + + (((x305) * (x308))) + + (((IkReal(-0.840000000000000)) * (x301)))); + evalcond[2] = + ((IkReal(-0.420000000000000)) + (((sj5) * (x308))) + + (((IkReal(-1.00000000000000)) * (x301))) + + (((IkReal(-1.00000000000000)) * (sj5) * (x309))) + + (((IkReal(-1.00000000000000)) * (x304)))); + evalcond[3] = + ((((IkReal(-0.420000000000000)) * (sj5))) + + (((IkReal(-1.00000000000000)) * (x302) * (x303))) + + (((IkReal(-1.00000000000000)) * (x309))) + (x308) + + (((IkReal(-1.00000000000000)) * (sj5) * (x304)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (cj5) * (x300) * + (x309))) + + (((cj5) * (sj4) * (x308))) + (((cj4) * (x310))) + + (((sj4) * (x306))) + (((cj4) * (x307)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x302) * (x309))) + + (((IkReal(-1.00000000000000)) * (x300) * (x310))) + + (((x302) * (x308))) + + (((IkReal(-1.00000000000000)) * (x300) * (x307))) + + (((cj4) * (x306))) + + (((IkReal(-1.00000000000000)) * (x303)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x311 = ((IkReal(49.0000000000000)) * (npx)); + IkReal x312 = ((IkReal(49.0000000000000)) * (npy)); + IkReal x313 = ((sj3) * (sj4) * (sj5)); + IkReal x314 = ((IkReal(100.000000000000)) * (cj5) * (npz)); + if (IKabs(((gconst0) * + (((((IkReal(-1.00000000000000)) * (npy) * (x314))) + + (((IkReal(-1.00000000000000)) * (x311) * (x313))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x312))) + + (((IkReal(-42.0000000000000)) * (npy))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst0) * + (((((IkReal(-1.00000000000000)) * (x312) * (x313))) + + (((npx) * (x314))) + (((cj3) * (x311))) + + (((IkReal(42.0000000000000)) * (npx))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((gconst0) * + (((((IkReal(-1.00000000000000)) * (npy) * (x314))) + + (((IkReal(-1.00000000000000)) * (x311) * (x313))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x312))) + + (((IkReal(-42.0000000000000)) * (npy)))))), + ((gconst0) * + (((((IkReal(-1.00000000000000)) * (x312) * (x313))) + + (((npx) * (x314))) + (((cj3) * (x311))) + + (((IkReal(42.0000000000000)) * (npx))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[6]; + IkReal x315 = IKcos(j6); + IkReal x316 = IKsin(j6); + IkReal x317 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x318 = ((cj5) * (npz)); + IkReal x319 = ((cj4) * (cj5)); + IkReal x320 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x321 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x322 = ((IkReal(0.840000000000000)) * (sj5)); + IkReal x323 = ((npz) * (sj5)); + IkReal x324 = ((npy) * (x315)); + IkReal x325 = ((npx) * (x315)); + IkReal x326 = ((npy) * (x316)); + IkReal x327 = ((npx) * (x316)); + evalcond[0] = ((((sj4) * (x320))) + (x324) + (x327)); + evalcond[1] = + ((IkReal(0.0637000000000000)) + (((x322) * (x325))) + + (((IkReal(-0.840000000000000)) * (x318))) + + (((IkReal(-1.00000000000000)) * (x322) * (x326))) + + (((IkReal(-1.00000000000000)) * (pp)))); + evalcond[2] = + ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x321))) + + (((sj5) * (x325))) + + (((IkReal(-1.00000000000000)) * (sj5) * (x326))) + + (((IkReal(-1.00000000000000)) * (x318)))); + evalcond[3] = + ((((IkReal(-0.420000000000000)) * (sj5))) + + (((IkReal(-1.00000000000000)) * (x326))) + (x325) + + (((IkReal(-1.00000000000000)) * (x319) * (x320))) + + (((IkReal(-1.00000000000000)) * (sj5) * (x321)))); + evalcond[4] = + ((((cj5) * (sj4) * (x325))) + (((sj4) * (x323))) + + (((IkReal(-1.00000000000000)) * (cj5) * (x317) * (x326))) + + (((cj4) * (x324))) + (((cj4) * (x327)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x317) * (x327))) + + (((IkReal(-1.00000000000000)) * (x317) * (x324))) + + (((IkReal(-1.00000000000000)) * (x320))) + + (((cj4) * (x323))) + + (((IkReal(-1.00000000000000)) * (x319) * (x326))) + + (((x319) * (x325)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j6array[2], cj6array[2], sj6array[2]; + bool j6valid[2] = {false}; + _nj6 = 2; + if (IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x328 = ((IkReal(1.00000000000000)) * (IKatan2(npy, npx))); + if (((((npx) * (npx)) + ((npy) * (npy)))) < (IkReal)-0.00001) + continue; + if ((((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) < -1 - IKFAST_SINCOS_THRESH || + (((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x329 = IKasin( + ((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))); + j6array[0] = ((((IkReal(-1.00000000000000)) * (x328))) + + (((IkReal(-1.00000000000000)) * (x329)))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + j6array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x328))) + (x329)); + sj6array[1] = IKsin(j6array[1]); + cj6array[1] = IKcos(j6array[1]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + if (j6array[1] > IKPI) + { + j6array[1] -= IK2PI; + } + else if (j6array[1] < -IKPI) + { + j6array[1] += IK2PI; + } + j6valid[1] = true; + for (int ij6 = 0; ij6 < 2; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 2; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x330 = (npx) * (npx); + IkReal x331 = (cj4) * (cj4); + IkReal x332 = (sj4) * (sj4); + IkReal x333 = IKcos(j6); + IkReal x334 = (npy) * (npy); + IkReal x335 = IKsin(j6); + IkReal x336 = ((npx) * (npy)); + IkReal x337 = ((IkReal(0.490000000000000)) * (sj3) * (sj4)); + IkReal x338 = ((IkReal(1.00000000000000)) * (x334)); + evalcond[0] = ((((x333) * (((((x331) * (x336))) + (((x332) * (x336))))))) + + (((x335) * (((((x330) * (x331))) + (((x330) * (x332))))))) + + (((npx) * (x337)))); + evalcond[1] = + ((((x335) * (((((IkReal(-1.00000000000000)) * (x332) * (x336))) + + (((IkReal(-1.00000000000000)) * (x331) * (x336))))))) + + (((x333) * (((((IkReal(-1.00000000000000)) * (x331) * (x338))) + + (((IkReal(-1.00000000000000)) * (x332) * (x338))))))) + + (((IkReal(-1.00000000000000)) * (npy) * (x337)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst10; + IkReal x339 = ((cj6) * (npx)); + IkReal x340 = ((IkReal(49.0000000000000)) * (cj3)); + IkReal x341 = ((npy) * (sj6)); + gconst10 = + IKsign(((((IkReal(-42.0000000000000)) * (x339))) + + (((IkReal(49.0000000000000)) * (cj4) * (npz) * (sj3))) + + (((x340) * (x341))) + (((IkReal(42.0000000000000)) * (x341))) + + (((IkReal(-1.00000000000000)) * (x339) * (x340))))); + IkReal x342 = ((npy) * (sj6)); + IkReal x343 = ((cj6) * (npx)); + IkReal x344 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = + ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + (x342) + + (((IkReal(-1.00000000000000)) * (x343) * (x344))) + + (((IkReal(-1.00000000000000)) * (x343))) + (((x342) * (x344)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst11; + IkReal x345 = ((npy) * (sj6)); + IkReal x346 = ((IkReal(1029.00000000000)) * (cj3)); + IkReal x347 = ((cj6) * (npx)); + gconst11 = + IKsign(((((x345) * (x346))) + + (((IkReal(1029.00000000000)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x346) * (x347))) + + (((IkReal(-882.000000000000)) * (x347))) + + (((IkReal(882.000000000000)) * (x345))))); + IkReal x348 = ((npy) * (sj6)); + IkReal x349 = ((cj6) * (npx)); + IkReal x350 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = + ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x349) * (x350))) + (x348) + + (((x348) * (x350))) + (((IkReal(-1.00000000000000)) * (x349)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x351 = ((cj4) * (sj3)); + IkReal x352 = ((IkReal(1225.00000000000)) * (pp)); + IkReal x353 = ((IkReal(2100.00000000000)) * (npz)); + if (IKabs(((gconst11) * + (((IkReal(66.8850000000000)) + + (((IkReal(-1.00000000000000)) * (cj3) * (x352))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(78.0325000000000)) * (cj3))) + + (((npz) * (x353))))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst11) * + (((((IkReal(78.0325000000000)) * (x351))) + + (((cj6) * (npx) * (x353))) + + (((IkReal(-1.00000000000000)) * (x351) * (x352))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * + (x353))))))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst11) * + (((IkReal(66.8850000000000)) + + (((IkReal(-1.00000000000000)) * (cj3) * (x352))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(78.0325000000000)) * (cj3))) + + (((npz) * (x353)))))), + ((gconst11) * + (((((IkReal(78.0325000000000)) * (x351))) + + (((cj6) * (npx) * (x353))) + + (((IkReal(-1.00000000000000)) * (x351) * (x352))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (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[6]; + IkReal x354 = IKcos(j5); + IkReal x355 = IKsin(j5); + IkReal x356 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x357 = ((cj6) * (sj4)); + IkReal x358 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x359 = ((sj4) * (sj6)); + IkReal x360 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x361 = ((cj6) * (npx)); + IkReal x362 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x363 = ((npz) * (x355)); + IkReal x364 = ((cj4) * (x354)); + IkReal x365 = ((sj6) * (x355)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x354) * (x362))) + + (((IkReal(-0.420000000000000)) * (x354))) + + (((IkReal(-1.00000000000000)) * (x360))) + + (((cj4) * (x355) * (x358)))); + evalcond[1] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-0.840000000000000)) * (npz) * (x354))) + + (((IkReal(0.840000000000000)) * (x355) * (x361))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x365)))); + evalcond[2] = + ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x354) * (x360))) + + (((IkReal(-1.00000000000000)) * (x362))) + + (((x355) * (x361))) + + (((IkReal(-1.00000000000000)) * (x356) * (x365)))); + evalcond[3] = + ((((IkReal(-0.420000000000000)) * (x355))) + + (((IkReal(-1.00000000000000)) * (x358) * (x364))) + + (((IkReal(-1.00000000000000)) * (x355) * (x362))) + + (x361) + (((IkReal(-1.00000000000000)) * (sj6) * (x356)))); + evalcond[4] = + ((((sj4) * (x363))) + (((npx) * (x354) * (x357))) + + (((cj4) * (npx) * (sj6))) + (((cj4) * (cj6) * (npy))) + + (((IkReal(-1.00000000000000)) * (x354) * (x356) * + (x359)))); + evalcond[5] = + ((((x361) * (x364))) + + (((IkReal(-1.00000000000000)) * (x358))) + + (((IkReal(-1.00000000000000)) * (x356) * (x357))) + + (((cj4) * (x363))) + + (((IkReal(-1.00000000000000)) * (npx) * (x359))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x356) * (x364)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x366 = ((cj4) * (sj3)); + IkReal x367 = ((IkReal(100.000000000000)) * (npz)); + if (IKabs(( + (gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x367))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3)))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(( + (gconst10) * + (((((IkReal(-20.5800000000000)) * (x366))) + + (((cj6) * (npx) * (x367))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x366))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x367))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x367))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3))))))), + ((gconst10) * + (((((IkReal(-20.5800000000000)) * (x366))) + + (((cj6) * (npx) * (x367))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x366))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x367))))))); + 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 x368 = IKcos(j5); + IkReal x369 = IKsin(j5); + IkReal x370 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x371 = ((cj6) * (sj4)); + IkReal x372 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x373 = ((sj4) * (sj6)); + IkReal x374 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x375 = ((cj6) * (npx)); + IkReal x376 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x377 = ((npz) * (x369)); + IkReal x378 = ((cj4) * (x368)); + IkReal x379 = ((sj6) * (x369)); + evalcond[0] = ((((IkReal(-1.00000000000000)) * (x368) * (x376))) + + (((cj4) * (x369) * (x372))) + + (((IkReal(-1.00000000000000)) * (x374))) + + (((IkReal(-0.420000000000000)) * (x368)))); + evalcond[1] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (x369) * (x375))) + + (((IkReal(-0.840000000000000)) * (npz) * (x368))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x379)))); + evalcond[2] = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x368) * (x374))) + + (((IkReal(-1.00000000000000)) * (x370) * (x379))) + + (((x369) * (x375))) + + (((IkReal(-1.00000000000000)) * (x376)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (x372) * (x378))) + + (x375) + (((IkReal(-0.420000000000000)) * (x369))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x370))) + + (((IkReal(-1.00000000000000)) * (x369) * (x376)))); + evalcond[4] = + ((((sj4) * (x377))) + + (((IkReal(-1.00000000000000)) * (x368) * (x370) * (x373))) + + (((cj4) * (npx) * (sj6))) + (((cj4) * (cj6) * (npy))) + + (((npx) * (x368) * (x371)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (npx) * (x373))) + + (((x375) * (x378))) + + (((IkReal(-1.00000000000000)) * (x370) * (x371))) + + (((IkReal(-1.00000000000000)) * (x372))) + (((cj4) * (x377))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x370) * (x378)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j6array[2], cj6array[2], sj6array[2]; + bool j6valid[2] = {false}; + _nj6 = 2; + if (IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x380 = ((IkReal(1.00000000000000)) * (IKatan2(npy, npx))); + if (((((npx) * (npx)) + ((npy) * (npy)))) < (IkReal)-0.00001) + continue; + if ((((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) < -1 - IKFAST_SINCOS_THRESH || + (((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x381 = IKasin( + ((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))); + j6array[0] = ((((IkReal(-1.00000000000000)) * (x381))) + + (((IkReal(-1.00000000000000)) * (x380)))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + j6array[1] = ((IkReal(3.14159265358979)) + (x381) + + (((IkReal(-1.00000000000000)) * (x380)))); + sj6array[1] = IKsin(j6array[1]); + cj6array[1] = IKcos(j6array[1]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + if (j6array[1] > IKPI) + { + j6array[1] -= IK2PI; + } + else if (j6array[1] < -IKPI) + { + j6array[1] += IK2PI; + } + j6valid[1] = true; + for (int ij6 = 0; ij6 < 2; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 2; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + + { + IkReal dummyeval[1]; + IkReal gconst10; + IkReal x382 = ((cj6) * (npx)); + IkReal x383 = ((IkReal(49.0000000000000)) * (cj3)); + IkReal x384 = ((npy) * (sj6)); + gconst10 = + IKsign(((((IkReal(-42.0000000000000)) * (x382))) + + (((IkReal(42.0000000000000)) * (x384))) + (((x383) * (x384))) + + (((IkReal(49.0000000000000)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x382) * (x383))))); + IkReal x385 = ((npy) * (sj6)); + IkReal x386 = ((cj6) * (npx)); + IkReal x387 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = + ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + (x385) + + (((IkReal(-1.00000000000000)) * (x386))) + + (((IkReal(-1.00000000000000)) * (x386) * (x387))) + (((x385) * (x387)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst11; + IkReal x388 = ((npy) * (sj6)); + IkReal x389 = ((IkReal(1029.00000000000)) * (cj3)); + IkReal x390 = ((cj6) * (npx)); + gconst11 = + IKsign(((((IkReal(-1.00000000000000)) * (x389) * (x390))) + + (((IkReal(-882.000000000000)) * (x390))) + (((x388) * (x389))) + + (((IkReal(1029.00000000000)) * (cj4) * (npz) * (sj3))) + + (((IkReal(882.000000000000)) * (x388))))); + IkReal x391 = ((npy) * (sj6)); + IkReal x392 = ((cj6) * (npx)); + IkReal x393 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x392))) + (x391) + + (((x391) * (x393))) + + (((IkReal(-1.00000000000000)) * (x392) * (x393)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x394 = ((cj4) * (sj3)); + IkReal x395 = ((IkReal(1225.00000000000)) * (pp)); + IkReal x396 = ((IkReal(2100.00000000000)) * (npz)); + if (IKabs(((gconst11) * + (((IkReal(66.8850000000000)) + (((npz) * (x396))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x395))) + + (((IkReal(78.0325000000000)) * (cj3))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(( + (gconst11) * + (((((cj6) * (npx) * (x396))) + + (((IkReal(-1.00000000000000)) * (x394) * (x395))) + + (((IkReal(78.0325000000000)) * (x394))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x396))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst11) * (((IkReal(66.8850000000000)) + (((npz) * (x396))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x395))) + + (((IkReal(78.0325000000000)) * (cj3)))))), + ((gconst11) * + (((((cj6) * (npx) * (x396))) + + (((IkReal(-1.00000000000000)) * (x394) * (x395))) + + (((IkReal(78.0325000000000)) * (x394))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x396))))))); + 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 x397 = IKcos(j5); + IkReal x398 = IKsin(j5); + IkReal x399 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x400 = ((cj6) * (sj4)); + IkReal x401 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x402 = ((sj4) * (sj6)); + IkReal x403 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x404 = ((cj6) * (npx)); + IkReal x405 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x406 = ((npz) * (x398)); + IkReal x407 = ((cj4) * (x397)); + IkReal x408 = ((sj6) * (x398)); + evalcond[0] = ((((cj4) * (x398) * (x401))) + + (((IkReal(-1.00000000000000)) * (x397) * (x405))) + + (((IkReal(-0.420000000000000)) * (x397))) + + (((IkReal(-1.00000000000000)) * (x403)))); + evalcond[1] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (x398) * (x404))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x408))) + + (((IkReal(-0.840000000000000)) * (npz) * (x397)))); + evalcond[2] = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x399) * (x408))) + + (((IkReal(-1.00000000000000)) * (x397) * (x403))) + + (((x398) * (x404))) + + (((IkReal(-1.00000000000000)) * (x405)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x401) * (x407))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x399))) + (x404) + + (((IkReal(-1.00000000000000)) * (x398) * (x405))) + + (((IkReal(-0.420000000000000)) * (x398)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x397) * (x399) * (x402))) + + (((npx) * (x397) * (x400))) + (((cj4) * (npx) * (sj6))) + + (((cj4) * (cj6) * (npy))) + (((sj4) * (x406)))); + evalcond[5] = + ((((x404) * (x407))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x399) * (x407))) + + (((cj4) * (x406))) + + (((IkReal(-1.00000000000000)) * (x399) * (x400))) + + (((IkReal(-1.00000000000000)) * (npx) * (x402))) + + (((IkReal(-1.00000000000000)) * (x401)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x409 = ((cj4) * (sj3)); + IkReal x410 = ((IkReal(100.000000000000)) * (npz)); + if (IKabs(((gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x410))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3)))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst10) * + (((((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x410))) + + (((cj6) * (npx) * (x410))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x409))) + + (((IkReal(-20.5800000000000)) * (x409))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x410))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3))))))), + ((gconst10) * + (((((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x410))) + + (((cj6) * (npx) * (x410))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x409))) + + (((IkReal(-20.5800000000000)) * (x409))))))); + 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 x411 = IKcos(j5); + IkReal x412 = IKsin(j5); + IkReal x413 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x414 = ((cj6) * (sj4)); + IkReal x415 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x416 = ((sj4) * (sj6)); + IkReal x417 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x418 = ((cj6) * (npx)); + IkReal x419 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x420 = ((npz) * (x412)); + IkReal x421 = ((cj4) * (x411)); + IkReal x422 = ((sj6) * (x412)); + evalcond[0] = ((((cj4) * (x412) * (x415))) + + (((IkReal(-0.420000000000000)) * (x411))) + + (((IkReal(-1.00000000000000)) * (x411) * (x419))) + + (((IkReal(-1.00000000000000)) * (x417)))); + evalcond[1] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (x412) * (x418))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x422))) + + (((IkReal(-0.840000000000000)) * (npz) * (x411)))); + evalcond[2] = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x411) * (x417))) + + (((x412) * (x418))) + + (((IkReal(-1.00000000000000)) * (x413) * (x422))) + + (((IkReal(-1.00000000000000)) * (x419)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (sj6) * (x413))) + + (((IkReal(-1.00000000000000)) * (x412) * (x419))) + + (((IkReal(-0.420000000000000)) * (x412))) + (x418) + + (((IkReal(-1.00000000000000)) * (x415) * (x421)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x411) * (x413) * (x416))) + + (((npx) * (x411) * (x414))) + (((sj4) * (x420))) + + (((cj4) * (npx) * (sj6))) + (((cj4) * (cj6) * (npy)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (sj6) * (x413) * (x421))) + + (((x418) * (x421))) + (((cj4) * (x420))) + + (((IkReal(-1.00000000000000)) * (x413) * (x414))) + + (((IkReal(-1.00000000000000)) * (npx) * (x416))) + + (((IkReal(-1.00000000000000)) * (x415)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + } + } + return solutions.GetNumSolutions() > 0; + } + inline void rotationfunction0(IkSolutionListBase& solutions) + { + for (int rotationiter = 0; rotationiter < 1; ++rotationiter) + { + IkReal x78 = ((cj6) * (sj4)); + IkReal x79 = ((IkReal(1.00000000000000)) * (sj5)); + IkReal x80 = ((cj4) * (cj5)); + IkReal x81 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x82 = ((cj3) * (sj5)); + IkReal x83 = ((IkReal(1.00000000000000)) * (sj6)); + IkReal x84 = ((sj4) * (sj6)); + IkReal x85 = ((IkReal(1.00000000000000)) * (cj3)); + IkReal x86 = ((IkReal(-1.00000000000000)) * (cj3)); + IkReal x87 = ((((cj3) * (x80))) + (((IkReal(-1.00000000000000)) * (sj3) * (x79)))); + IkReal x88 = ((((cj5) * (sj3))) + (((cj4) * (x82)))); + IkReal x89 = ((((cj5) * (x84))) + (((IkReal(-1.00000000000000)) * (cj4) * (cj6)))); + IkReal x90 = ((x82) + (((sj3) * (x80)))); + IkReal x91 = ((((cj4) * (sj3) * (sj5))) + (((IkReal(-1.00000000000000)) * (cj5) * (x85)))); + IkReal x92 = ((cj6) * (x87)); + IkReal x93 = ((((IkReal(-1.00000000000000)) * (cj4) * (x83))) + + (((IkReal(-1.00000000000000)) * (cj5) * (x78)))); + IkReal x94 = ((((IkReal(-1.00000000000000)) * (cj3) * (sj4) * (x83))) + (x92)); + IkReal x95 = ((((IkReal(-1.00000000000000)) * (x81) * (x84))) + (((cj6) * (x90)))); + IkReal x96 = ((((IkReal(-1.00000000000000)) * (x83) * (x90))) + + (((IkReal(-1.00000000000000)) * (x78) * (x81)))); + IkReal x97 = ((((x78) * (x86))) + (((IkReal(-1.00000000000000)) * (sj6) * (x87)))); + new_r00 = ((((r00) * (((((x84) * (x86))) + (x92))))) + + (((r01) * (((((IkReal(-1.00000000000000)) * (x83) * (x87))) + + (((IkReal(-1.00000000000000)) * (x78) * (x85))))))) + + (((r02) * (x88)))); + new_r01 = ((((r12) * (x88))) + (((r11) * (x97))) + (((r10) * (x94)))); + new_r02 = ((((r21) * (x97))) + (((r20) * (x94))) + (((r22) * (x88)))); + new_r10 = ((((r00) * (x93))) + (((r01) * (x89))) + + (((IkReal(-1.00000000000000)) * (r02) * (sj4) * (x79)))); + new_r11 = ((((r11) * (x89))) + (((r10) * (x93))) + + (((IkReal(-1.00000000000000)) * (r12) * (sj4) * (x79)))); + new_r12 = ((((r21) * (x89))) + (((IkReal(-1.00000000000000)) * (r22) * (sj4) * (x79))) + + (((r20) * (x93)))); + new_r20 = ((((r01) * (x96))) + (((r00) * (x95))) + (((r02) * (x91)))); + new_r21 = ((((r12) * (x91))) + (((r11) * (x96))) + (((r10) * (x95)))); + new_r22 = ((((r21) * (x96))) + (((r20) * (x95))) + (((r22) * (x91)))); + { + IkReal j1array[2], cj1array[2], sj1array[2]; + bool j1valid[2] = {false}; + _nj1 = 2; + cj1array[0] = new_r22; + if (cj1array[0] >= -1 - IKFAST_SINCOS_THRESH && cj1array[0] <= 1 + IKFAST_SINCOS_THRESH) + { + j1valid[0] = j1valid[1] = true; + j1array[0] = IKacos(cj1array[0]); + sj1array[0] = IKsin(j1array[0]); + cj1array[1] = cj1array[0]; + j1array[1] = -j1array[0]; + sj1array[1] = -sj1array[0]; + } + else if (isnan(cj1array[0])) + { + // probably any value will work + j1valid[0] = true; + cj1array[0] = 1; + sj1array[0] = 0; + j1array[0] = 0; + } + for (int ij1 = 0; ij1 < 2; ++ij1) + { + if (!j1valid[ij1]) + { + continue; + } + _ij1[0] = ij1; + _ij1[1] = -1; + for (int iij1 = ij1 + 1; iij1 < 2; ++iij1) + { + if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH && + IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH) + { + j1valid[iij1] = false; + _ij1[1] = iij1; + break; + } + } + j1 = j1array[ij1]; + cj1 = cj1array[ij1]; + sj1 = sj1array[ij1]; + + { + IkReal dummyeval[1]; + IkReal gconst12; + gconst12 = IKsign(sj1); + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst13; + gconst13 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst14; + gconst14 = IKsign(((((cj1) * ((new_r12) * (new_r12)))) + + (((cj1) * ((new_r02) * (new_r02)))))); + dummyeval[0] = + ((((cj1) * ((new_r12) * (new_r12)))) + (((cj1) * ((new_r02) * (new_r02))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[7]; + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.57079632679490)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = new_r22; + evalcond[2] = new_r22; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(new_r21) + IKsqr(new_r20) - 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2(new_r21, new_r20); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[2]; + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (IKcos(j0)))) + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (IKsin(j0)))) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst21; + gconst21 = + IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst20; + gconst20 = IKsign( + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x98 = ((gconst20) * (sj0)); + if (IKabs(((new_r12) * (x98))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * + (x98))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2( + ((new_r12) * (x98)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x98))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x99 = IKsin(j2); + IkReal x100 = IKcos(j2); + IkReal x101 = ((IkReal(1.00000000000000)) * (x99)); + evalcond[0] = + ((((new_r02) * (x99))) + (((new_r12) * (x100)))); + evalcond[1] = ((((new_r10) * (x100))) + (sj0) + + (((new_r00) * (x99)))); + evalcond[2] = ((IkReal(1.00000000000000)) + + (((new_r02) * (x100))) + + (((IkReal(-1.00000000000000)) * + (new_r12) * (x101)))); + evalcond[3] = ((((new_r01) * (x99))) + + (((IkReal(-1.00000000000000)) * (cj0))) + + (((new_r11) * (x100)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * + (new_r10) * (x101))) + + (((new_r00) * (x100)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * + (new_r11) * (x101))) + + (((new_r01) * (x100)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + if (IKabs(((gconst21) * (new_r12))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst21) * + (new_r02))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2( + ((gconst21) * (new_r12)), + ((IkReal(-1.00000000000000)) * (gconst21) * (new_r02))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x102 = IKsin(j2); + IkReal x103 = IKcos(j2); + IkReal x104 = ((IkReal(1.00000000000000)) * (x102)); + evalcond[0] = + ((((new_r02) * (x102))) + (((new_r12) * (x103)))); + evalcond[1] = ((((new_r10) * (x103))) + (sj0) + + (((new_r00) * (x102)))); + evalcond[2] = + ((IkReal(1.00000000000000)) + (((new_r02) * (x103))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (x104)))); + evalcond[3] = ((((new_r01) * (x102))) + + (((IkReal(-1.00000000000000)) * (cj0))) + + (((new_r11) * (x103)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (new_r10) * (x104))) + + (((new_r00) * (x103)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (new_r11) * (x104))) + + (((new_r01) * (x103)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + else + { + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(4.71238898038469)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = new_r22; + evalcond[2] = ((IkReal(-1.00000000000000)) * (new_r22)); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((IkReal(-1.00000000000000)) * (new_r21))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r20))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((IkReal(-1.00000000000000)) * (new_r21))) + + IKsqr(((IkReal(-1.00000000000000)) * (new_r20))) - 1) <= + IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2(((IkReal(-1.00000000000000)) * (new_r21)), + ((IkReal(-1.00000000000000)) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[2]; + evalcond[0] = ((IKcos(j0)) + (new_r20)); + evalcond[1] = ((IKsin(j0)) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst24; + gconst24 = IKsign( + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst25; + gconst25 = IKsign(((((IkReal(-1.00000000000000)) * + ((new_r02) * (new_r02)))) + + (((IkReal(-1.00000000000000)) * + ((new_r12) * (new_r12)))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * + ((new_r02) * (new_r02)))) + + (((IkReal(-1.00000000000000)) * + ((new_r12) * (new_r12))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + if (IKabs(((gconst25) * (new_r12))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst25) * + (new_r02))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((gconst25) * (new_r12)), + ((IkReal(-1.00000000000000)) * + (gconst25) * (new_r02))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x105 = IKsin(j2); + IkReal x106 = IKcos(j2); + IkReal x107 = ((IkReal(1.00000000000000)) * (x105)); + evalcond[0] = + ((((new_r02) * (x105))) + (((new_r12) * (x106)))); + evalcond[1] = ((((new_r10) * (x106))) + (sj0) + + (((new_r00) * (x105)))); + evalcond[2] = ((IkReal(-1.00000000000000)) + + (((new_r02) * (x106))) + + (((IkReal(-1.00000000000000)) * + (new_r12) * (x107)))); + evalcond[3] = + ((((new_r01) * (x105))) + + (((IkReal(-1.00000000000000)) * (cj0))) + + (((new_r11) * (x106)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * + (new_r10) * (x107))) + + (((new_r00) * (x106)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * + (new_r11) * (x107))) + + (((new_r01) * (x106)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos( + 7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x108 = ((gconst24) * (sj0)); + if (IKabs(((new_r12) * (x108))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * + (x108))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2( + ((new_r12) * (x108)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x108))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x109 = IKsin(j2); + IkReal x110 = IKcos(j2); + IkReal x111 = ((IkReal(1.00000000000000)) * (x109)); + evalcond[0] = + ((((new_r02) * (x109))) + (((new_r12) * (x110)))); + evalcond[1] = ((((new_r10) * (x110))) + (sj0) + + (((new_r00) * (x109)))); + evalcond[2] = ((IkReal(-1.00000000000000)) + + (((new_r02) * (x110))) + + (((IkReal(-1.00000000000000)) * (new_r12) * + (x111)))); + evalcond[3] = + ((((new_r11) * (x110))) + (((new_r01) * (x109))) + + (((IkReal(-1.00000000000000)) * (cj0)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * (new_r10) * + (x111))) + + (((new_r00) * (x110)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * (new_r11) * + (x111))) + + (((new_r01) * (x110)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + else + { + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[5] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[6] = ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000) + { + { + IkReal j2array[2], cj2array[2], sj2array[2]; + bool j2valid[2] = {false}; + _nj2 = 2; + if (IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x112 = IKatan2(new_r12, new_r02); + j2array[0] = ((IkReal(-1.00000000000000)) * (x112)); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + j2array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x112)))); + sj2array[1] = IKsin(j2array[1]); + cj2array[1] = IKcos(j2array[1]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + if (j2array[1] > IKPI) + { + j2array[1] -= IK2PI; + } + else if (j2array[1] < -IKPI) + { + j2array[1] += IK2PI; + } + j2valid[1] = true; + 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 evalcond[1]; + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (new_r12) * (IKsin(j2)))) + + (((new_r02) * (IKcos(j2))))); + if (IKabs(evalcond[0]) > 0.000001) + { + continue; + } + } + + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs( + IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x113 = IKsin(j0); + IkReal x114 = ((IkReal(1.00000000000000)) * (sj2)); + IkReal x115 = ((IkReal(1.00000000000000)) * (IKcos(j0))); + evalcond[0] = ((((new_r00) * (sj2))) + (x113) + + (((cj2) * (new_r10)))); + evalcond[1] = ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x115))) + + (((cj2) * (new_r11)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r10) * (x114))) + + (((IkReal(-1.00000000000000)) * (x115))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (new_r11) * (x114))) + + (((cj2) * (new_r01))) + + (((IkReal(-1.00000000000000)) * (x113)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + IkReal x116 = ((IkReal(1.00000000000000)) + (new_r22)); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = x116; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = new_r20; + evalcond[5] = new_r21; + evalcond[6] = x116; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000) + { + { + IkReal j2array[2], cj2array[2], sj2array[2]; + bool j2valid[2] = {false}; + _nj2 = 2; + if (IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x117 = IKatan2(new_r12, new_r02); + j2array[0] = ((IkReal(-1.00000000000000)) * (x117)); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + j2array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x117)))); + sj2array[1] = IKsin(j2array[1]); + cj2array[1] = IKcos(j2array[1]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + if (j2array[1] > IKPI) + { + j2array[1] -= IK2PI; + } + else if (j2array[1] < -IKPI) + { + j2array[1] += IK2PI; + } + j2valid[1] = true; + 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 evalcond[1]; + evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r12) * + (IKsin(j2)))) + + (((new_r02) * (IKcos(j2))))); + if (IKabs(evalcond[0]) > 0.000001) + { + continue; + } + } + + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x118 = IKcos(j0); + IkReal x119 = IKsin(j0); + IkReal x120 = ((IkReal(1.00000000000000)) * (sj2)); + evalcond[0] = ((((new_r00) * (sj2))) + (x119) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x118)))); + evalcond[2] = ((((IkReal(-1.00000000000000)) * (new_r10) * + (x120))) + + (x118) + (((cj2) * (new_r00)))); + evalcond[3] = ((x119) + (((cj2) * (new_r01))) + + (((IkReal(-1.00000000000000)) * (new_r11) * + (x120)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x121 = ((gconst14) * (new_r22) * (sj1)); + if (IKabs(((new_r12) * (x121))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x121))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((new_r12) * (x121)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x121))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x122 = IKcos(j2); + IkReal x123 = IKsin(j2); + IkReal x124 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x125 = ((sj1) * (x122)); + IkReal x126 = ((new_r12) * (x123)); + IkReal x127 = ((new_r02) * (x122)); + IkReal x128 = ((IkReal(1.00000000000000)) * (sj1) * (x123)); + evalcond[0] = ((((new_r02) * (x123))) + (((new_r12) * (x122)))); + evalcond[1] = + ((sj1) + (x127) + (((IkReal(-1.00000000000000)) * (x126)))); + evalcond[2] = ((((cj1) * (x127))) + + (((IkReal(-1.00000000000000)) * (x124) * (x126))) + + (((new_r22) * (sj1)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (new_r10) * (x128))) + + (((new_r00) * (x125))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x124)))); + evalcond[4] = ((((new_r01) * (x125))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x124))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x128)))); + evalcond[5] = ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (sj1) * (x126))) + + (((new_r02) * (x125))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x124)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst15; + gconst15 = IKsign(sj1); + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[11]; + IkReal x129 = + ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + IkReal x130 = + ((((cj2) * (new_r02))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj2)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x129; + evalcond[5] = x129; + evalcond[6] = x130; + evalcond[7] = x130; + evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[9] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[10] = + ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * + (new_r00) * (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x131 = IKsin(j0); + IkReal x132 = ((IkReal(1.00000000000000)) * (sj2)); + IkReal x133 = + ((IkReal(1.00000000000000)) * (IKcos(j0))); + evalcond[0] = ((((new_r00) * (sj2))) + (x131) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x133))) + + (((cj2) * (new_r11)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r10) * + (x132))) + + (((IkReal(-1.00000000000000)) * (x133))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (new_r11) * + (x132))) + + (((cj2) * (new_r01))) + + (((IkReal(-1.00000000000000)) * (x131)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > + vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + IkReal x134 = ((IkReal(1.00000000000000)) + (new_r22)); + IkReal x135 = ((new_r12) * (sj2)); + IkReal x136 = ((cj2) * (new_r02)); + IkReal x137 = + ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = x134; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x137; + evalcond[5] = x137; + evalcond[6] = + ((x136) + (((IkReal(-1.00000000000000)) * (x135)))); + evalcond[7] = + ((x135) + (((IkReal(-1.00000000000000)) * (x136)))); + evalcond[8] = new_r20; + evalcond[9] = new_r21; + evalcond[10] = x134; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * + (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * + (new_r00) * (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x138 = IKcos(j0); + IkReal x139 = IKsin(j0); + IkReal x140 = + ((IkReal(1.00000000000000)) * (sj2)); + evalcond[0] = ((((new_r00) * (sj2))) + (x139) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + + (((cj2) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x138)))); + evalcond[2] = + ((x138) + (((IkReal(-1.00000000000000)) * + (new_r10) * (x140))) + + (((cj2) * (new_r00)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * + (new_r11) * (x140))) + + (x139) + (((cj2) * (new_r01)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > + vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((new_r20) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((new_r20) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((new_r20) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x141 = IKsin(j0); + IkReal x142 = IKcos(j0); + IkReal x143 = ((cj2) * (new_r01)); + IkReal x144 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x145 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x146 = ((new_r10) * (sj2)); + IkReal x147 = ((new_r11) * (sj2)); + IkReal x148 = ((cj2) * (new_r00)); + IkReal x149 = ((IkReal(1.00000000000000)) * (x142)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x142) * (x145))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x141) * (x145))) + + (new_r21)); + evalcond[2] = ((((new_r00) * (sj2))) + (x141) + + (((cj2) * (new_r10)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x149))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x142) * (x144))) + + (x148) + (((IkReal(-1.00000000000000)) * (x146)))); + evalcond[5] = + ((x143) + (((IkReal(-1.00000000000000)) * (x147))) + + (((IkReal(-1.00000000000000)) * (x141) * (x144)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x149))) + + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x144) * (x146))) + + (((cj1) * (x148)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x144) * (x147))) + + (((IkReal(-1.00000000000000)) * (x141))) + + (((new_r21) * (sj1))) + (((cj1) * (x143)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs( + ((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs( + IKsqr(((new_r21) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) + + IKsqr(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x150 = IKsin(j0); + IkReal x151 = IKcos(j0); + IkReal x152 = ((cj2) * (new_r01)); + IkReal x153 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x154 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x155 = ((new_r10) * (sj2)); + IkReal x156 = ((new_r11) * (sj2)); + IkReal x157 = ((cj2) * (new_r00)); + IkReal x158 = ((IkReal(1.00000000000000)) * (x151)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x151) * (x154))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x150) * (x154))) + + (new_r21)); + evalcond[2] = ((((new_r00) * (sj2))) + (x150) + + (((cj2) * (new_r10)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x158))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x151) * (x153))) + + (x157) + (((IkReal(-1.00000000000000)) * (x155)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x150) * (x153))) + + (x152) + (((IkReal(-1.00000000000000)) * (x156)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x158))) + + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x153) * (x155))) + + (((cj1) * (x157)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x153) * (x156))) + + (((IkReal(-1.00000000000000)) * (x150))) + + (((new_r21) * (sj1))) + (((cj1) * (x152)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((gconst15) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst15) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j0array[0] = + IKatan2(((gconst15) * (new_r21)), ((gconst15) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x159 = IKsin(j0); + IkReal x160 = IKcos(j0); + IkReal x161 = ((cj2) * (new_r01)); + IkReal x162 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x163 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x164 = ((new_r10) * (sj2)); + IkReal x165 = ((new_r11) * (sj2)); + IkReal x166 = ((cj2) * (new_r00)); + IkReal x167 = ((IkReal(1.00000000000000)) * (x160)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x160) * (x163))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x159) * (x163))) + + (new_r21)); + evalcond[2] = + ((((new_r00) * (sj2))) + (x159) + (((cj2) * (new_r10)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (x167))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x160) * (x162))) + + (x166) + (((IkReal(-1.00000000000000)) * (x164)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x159) * (x162))) + + (x161) + (((IkReal(-1.00000000000000)) * (x165)))); + evalcond[6] = + ((((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x167))) + + (((IkReal(-1.00000000000000)) * (x162) * (x164))) + + (((cj1) * (x166)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x162) * (x165))) + + (((IkReal(-1.00000000000000)) * (x159))) + + (((cj1) * (x161))) + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x168 = ((gconst13) * (sj1)); + if (IKabs(((new_r12) * (x168))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x168))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((new_r12) * (x168)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x168))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x169 = IKcos(j2); + IkReal x170 = IKsin(j2); + IkReal x171 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x172 = ((sj1) * (x169)); + IkReal x173 = ((new_r12) * (x170)); + IkReal x174 = ((new_r02) * (x169)); + IkReal x175 = ((IkReal(1.00000000000000)) * (sj1) * (x170)); + evalcond[0] = ((((new_r02) * (x170))) + (((new_r12) * (x169)))); + evalcond[1] = ((sj1) + (((IkReal(-1.00000000000000)) * (x173))) + (x174)); + evalcond[2] = ((((new_r22) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x171) * (x173))) + + (((cj1) * (x174)))); + evalcond[3] = ((((new_r00) * (x172))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x171))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x175)))); + evalcond[4] = ((((new_r01) * (x172))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x171))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x175)))); + evalcond[5] = ((IkReal(1.00000000000000)) + (((new_r02) * (x172))) + + (((IkReal(-1.00000000000000)) * (sj1) * (x173))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x171)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst15; + gconst15 = IKsign(sj1); + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[11]; + IkReal x176 = ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + IkReal x177 = + ((((cj2) * (new_r02))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj2)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x176; + evalcond[5] = x176; + evalcond[6] = x177; + evalcond[7] = x177; + evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[9] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[10] = ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x178 = IKsin(j0); + IkReal x179 = ((IkReal(1.00000000000000)) * (sj2)); + IkReal x180 = + ((IkReal(1.00000000000000)) * (IKcos(j0))); + evalcond[0] = ((((new_r00) * (sj2))) + (x178) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x180))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r10) * + (x179))) + + (((IkReal(-1.00000000000000)) * (x180))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x178))) + + (((IkReal(-1.00000000000000)) * (new_r11) * + (x179))) + + (((cj2) * (new_r01)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + IkReal x181 = ((IkReal(1.00000000000000)) + (new_r22)); + IkReal x182 = ((new_r12) * (sj2)); + IkReal x183 = ((cj2) * (new_r02)); + IkReal x184 = ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = x181; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x184; + evalcond[5] = x184; + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x182))) + (x183)); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x183))) + (x182)); + evalcond[8] = new_r20; + evalcond[9] = new_r21; + evalcond[10] = x181; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs( + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * + (new_r00) * (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x185 = IKcos(j0); + IkReal x186 = IKsin(j0); + IkReal x187 = ((IkReal(1.00000000000000)) * (sj2)); + evalcond[0] = ((((new_r00) * (sj2))) + (x186) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x185))) + + (((cj2) * (new_r11)))); + evalcond[2] = + ((x185) + (((IkReal(-1.00000000000000)) * + (new_r10) * (x187))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((x186) + (((IkReal(-1.00000000000000)) * + (new_r11) * (x187))) + + (((cj2) * (new_r01)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos( + 7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs( + ((new_r20) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((new_r20) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((new_r20) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x188 = IKsin(j0); + IkReal x189 = IKcos(j0); + IkReal x190 = ((cj2) * (new_r01)); + IkReal x191 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x192 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x193 = ((new_r10) * (sj2)); + IkReal x194 = ((new_r11) * (sj2)); + IkReal x195 = ((cj2) * (new_r00)); + IkReal x196 = ((IkReal(1.00000000000000)) * (x189)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x189) * (x192))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x188) * (x192))) + + (new_r21)); + evalcond[2] = ((((new_r00) * (sj2))) + (x188) + + (((cj2) * (new_r10)))); + evalcond[3] = ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x196))) + + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x189) * (x191))) + + (((IkReal(-1.00000000000000)) * (x193))) + (x195)); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x188) * (x191))) + + (((IkReal(-1.00000000000000)) * (x194))) + (x190)); + evalcond[6] = + ((((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x191) * (x193))) + + (((IkReal(-1.00000000000000)) * (x196))) + + (((cj1) * (x195)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x191) * (x194))) + + (((IkReal(-1.00000000000000)) * (x188))) + + (((cj1) * (x190))) + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((new_r21) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) + + IKsqr(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = + IKatan2(((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x197 = IKsin(j0); + IkReal x198 = IKcos(j0); + IkReal x199 = ((cj2) * (new_r01)); + IkReal x200 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x201 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x202 = ((new_r10) * (sj2)); + IkReal x203 = ((new_r11) * (sj2)); + IkReal x204 = ((cj2) * (new_r00)); + IkReal x205 = ((IkReal(1.00000000000000)) * (x198)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x198) * (x201))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x197) * (x201))) + + (new_r21)); + evalcond[2] = + ((((new_r00) * (sj2))) + (x197) + (((cj2) * (new_r10)))); + evalcond[3] = ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x205))) + + (((cj2) * (new_r11)))); + evalcond[4] = + ((x204) + + (((IkReal(-1.00000000000000)) * (x198) * (x200))) + + (((IkReal(-1.00000000000000)) * (x202)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x197) * (x200))) + + (x199) + (((IkReal(-1.00000000000000)) * (x203)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x200) * (x202))) + + (((cj1) * (x204))) + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x205)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x200) * (x203))) + + (((IkReal(-1.00000000000000)) * (x197))) + + (((cj1) * (x199))) + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((gconst15) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst15) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j0array[0] = + IKatan2(((gconst15) * (new_r21)), ((gconst15) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x206 = IKsin(j0); + IkReal x207 = IKcos(j0); + IkReal x208 = ((cj2) * (new_r01)); + IkReal x209 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x210 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x211 = ((new_r10) * (sj2)); + IkReal x212 = ((new_r11) * (sj2)); + IkReal x213 = ((cj2) * (new_r00)); + IkReal x214 = ((IkReal(1.00000000000000)) * (x207)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x207) * (x210))) + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x206) * (x210))) + (new_r21)); + evalcond[2] = + ((((new_r00) * (sj2))) + (x206) + (((cj2) * (new_r10)))); + evalcond[3] = ((((new_r01) * (sj2))) + (((cj2) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x214)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * (x207) * (x209))) + + (x213) + (((IkReal(-1.00000000000000)) * (x211)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * (x206) * (x209))) + + (x208) + (((IkReal(-1.00000000000000)) * (x212)))); + evalcond[6] = ((((new_r20) * (sj1))) + (((cj1) * (x213))) + + (((IkReal(-1.00000000000000)) * (x214))) + + (((IkReal(-1.00000000000000)) * (x209) * (x211)))); + evalcond[7] = ((((cj1) * (x208))) + (((new_r21) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x206))) + + (((IkReal(-1.00000000000000)) * (x209) * (x212)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((gconst12) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst12) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j0array[0] = IKatan2(((gconst12) * (new_r21)), ((gconst12) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[2]; + IkReal x215 = ((IkReal(1.00000000000000)) * (sj1)); + evalcond[0] = + ((new_r20) + (((IkReal(-1.00000000000000)) * (x215) * (IKcos(j0))))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x215) * (IKsin(j0)))) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst17; + gconst17 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst16; + gconst16 = IKsign(((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x216 = ((gconst16) * (sj0)); + if (IKabs(((new_r12) * (x216))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x216))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = + IKatan2(((new_r12) * (x216)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x216))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[12]; + IkReal x217 = IKsin(j2); + IkReal x218 = IKcos(j2); + IkReal x219 = ((IkReal(1.00000000000000)) * (cj0)); + IkReal x220 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x221 = ((IkReal(1.00000000000000)) * (x217)); + IkReal x222 = ((new_r01) * (x218)); + IkReal x223 = ((new_r00) * (x218)); + IkReal x224 = ((new_r02) * (x218)); + evalcond[0] = ((((new_r02) * (x217))) + (((new_r12) * (x218)))); + evalcond[1] = + ((sj0) + (((new_r00) * (x217))) + (((new_r10) * (x218)))); + evalcond[2] = + ((sj1) + (x224) + + (((IkReal(-1.00000000000000)) * (new_r12) * (x221)))); + evalcond[3] = ((((new_r01) * (x217))) + + (((IkReal(-1.00000000000000)) * (x219))) + + (((new_r11) * (x218)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (cj1) * (x219))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x221))) + (x223)); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (new_r11) * (x221))) + + (((IkReal(-1.00000000000000)) * (sj0) * (x220))) + (x222)); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (new_r12) * (x217) * (x220))) + + (((cj1) * (x224))) + (((new_r22) * (sj1)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (new_r10) * (sj1) * (x221))) + + (((sj1) * (x223))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x220)))); + evalcond[8] = + ((((sj1) * (x222))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (sj1) * (x221))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x220)))); + evalcond[9] = + ((IkReal(1.00000000000000)) + (((sj1) * (x224))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x220))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj1) * (x221)))); + evalcond[10] = + ((((new_r20) * (sj1))) + (((cj1) * (x223))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x217) * (x220))) + + (((IkReal(-1.00000000000000)) * (x219)))); + evalcond[11] = + ((((IkReal(-1.00000000000000)) * (sj0))) + (((cj1) * (x222))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x217) * (x220))) + + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001 || + IKabs(evalcond[8]) > 0.000001 || + IKabs(evalcond[9]) > 0.000001 || + IKabs(evalcond[10]) > 0.000001 || + IKabs(evalcond[11]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x225 = ((gconst17) * (sj1)); + if (IKabs(((new_r12) * (x225))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x225))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((new_r12) * (x225)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x225))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[12]; + IkReal x226 = IKsin(j2); + IkReal x227 = IKcos(j2); + IkReal x228 = ((IkReal(1.00000000000000)) * (cj0)); + IkReal x229 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x230 = ((IkReal(1.00000000000000)) * (x226)); + IkReal x231 = ((new_r01) * (x227)); + IkReal x232 = ((new_r00) * (x227)); + IkReal x233 = ((new_r02) * (x227)); + evalcond[0] = ((((new_r02) * (x226))) + (((new_r12) * (x227)))); + evalcond[1] = ((((new_r00) * (x226))) + (sj0) + (((new_r10) * (x227)))); + evalcond[2] = + ((sj1) + (((IkReal(-1.00000000000000)) * (new_r12) * (x230))) + + (x233)); + evalcond[3] = ((((new_r01) * (x226))) + (((new_r11) * (x227))) + + (((IkReal(-1.00000000000000)) * (x228)))); + evalcond[4] = + ((x232) + (((IkReal(-1.00000000000000)) * (cj1) * (x228))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x230)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (sj0) * (x229))) + (x231) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x230)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (new_r12) * (x226) * (x229))) + + (((cj1) * (x233))) + (((new_r22) * (sj1)))); + evalcond[7] = + ((((sj1) * (x232))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x229))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (sj1) * (x230)))); + evalcond[8] = + ((((sj1) * (x231))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (sj1) * (x230))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x229)))); + evalcond[9] = + ((IkReal(1.00000000000000)) + (((sj1) * (x233))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x229))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj1) * (x230)))); + evalcond[10] = + ((((cj1) * (x232))) + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x226) * (x229))) + + (((IkReal(-1.00000000000000)) * (x228)))); + evalcond[11] = + ((((cj1) * (x231))) + (((IkReal(-1.00000000000000)) * (sj0))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x226) * (x229))) + + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || + IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || + IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + } + } + } + } + } +}; -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x265=((gconst8)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x265))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x265))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x265)), ((IkReal(-49.0000000000000))*(npx)*(x265))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x266=IKcos(j6); -IkReal x267=IKsin(j6); -evalcond[0]=((((npx)*(x267)))+(((npy)*(x266)))); -evalcond[1]=((((npx)*(x266)))+(((IkReal(-1.00000000000000))*(npy)*(x267)))+(((IkReal(0.490000000000000))*(sj3)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) +/// 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) { -continue; -} -} - -rotationfunction0(solutions); -} + IKSolver solver; + return solver.ComputeIk(eetrans, eerot, pfree, solutions); } -} - -} - -} else -{ -IkReal x268=((IkReal(1.00000000000000))*(pp)); -IkReal x269=((IkReal(0.490000000000000))*(cj3)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x268)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=((IkReal(0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(x269)); -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x268)))); -evalcond[4]=((IkReal(-0.420000000000000))+(npz)+(((IkReal(-1.00000000000000))*(x269)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst9; -gconst9=IKsign(((((IkReal(-100.000000000000))*((npy)*(npy))))+(((IkReal(-100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((npy)*(npy))))+(((IkReal(-1.00000000000000))*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x270=((gconst9)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x270))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x270))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x270)), ((IkReal(-49.0000000000000))*(npx)*(x270))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x271=IKcos(j6); -IkReal x272=IKsin(j6); -evalcond[0]=((((npy)*(x271)))+(((npx)*(x272)))); -evalcond[1]=((((IkReal(-0.490000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(npy)*(x272)))+(((npx)*(x271)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) +IKFAST_API const char* GetKinematicsHash() { -continue; -} -} - -rotationfunction0(solutions); -} + return ""; } -} +IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } -} +#ifdef IKFAST_NAMESPACE +} // end namespace +#endif -} else -{ -if( 1 ) +#ifndef IKFAST_NO_MAIN +#include +#include +#ifdef IKFAST_NAMESPACE +using namespace IKFAST_NAMESPACE; +#endif +int main(int argc, char** argv) { -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x273=(sj4)*(sj4); -IkReal x274=((cj4)*(sj4)); -IkReal x275=((IkReal(49.0000000000000))*(npx)*(sj3)); -IkReal x276=((IkReal(49.0000000000000))*(npy)*(sj3)); -if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x275)))+(((x274)*(x276))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x276)))+(((IkReal(-1.00000000000000))*(x274)*(x275))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x275)))+(((x274)*(x276)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x276)))+(((IkReal(-1.00000000000000))*(x274)*(x275))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x277=IKcos(j6); -IkReal x278=IKsin(j6); -IkReal x279=((IkReal(1.00000000000000))*(sj4)); -IkReal x280=((IkReal(0.490000000000000))*(sj3)); -IkReal x281=((npy)*(x277)); -IkReal x282=((npx)*(x277)); -IkReal x283=((npx)*(x278)); -IkReal x284=((npy)*(x278)); -evalcond[0]=((x283)+(x281)+(((sj4)*(x280)))); -evalcond[1]=((x282)+(((IkReal(-1.00000000000000))*(x284)))+(((cj4)*(x280)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x279)*(x282)))+(((cj4)*(x283)))+(((cj4)*(x281)))+(((sj4)*(x284)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(x282)))+(((IkReal(-1.00000000000000))*(x280)))+(((IkReal(-1.00000000000000))*(x279)*(x281)))+(((IkReal(-1.00000000000000))*(x279)*(x283)))+(((cj4)*(x284)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x285=((IkReal(49.0000000000000))*(cj4)*(sj3)); -IkReal x286=((IkReal(49.0000000000000))*(sj3)*(sj4)); -if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(npx)*(x286)))+(((npy)*(x285))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(npy)*(x286)))+(((IkReal(-1.00000000000000))*(npx)*(x285))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(npx)*(x286)))+(((npy)*(x285)))))), ((gconst6)*(((((IkReal(-1.00000000000000))*(npy)*(x286)))+(((IkReal(-1.00000000000000))*(npx)*(x285))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x287=IKcos(j6); -IkReal x288=IKsin(j6); -IkReal x289=((IkReal(1.00000000000000))*(sj4)); -IkReal x290=((IkReal(0.490000000000000))*(sj3)); -IkReal x291=((npy)*(x287)); -IkReal x292=((npx)*(x287)); -IkReal x293=((npx)*(x288)); -IkReal x294=((npy)*(x288)); -evalcond[0]=((((sj4)*(x290)))+(x291)+(x293)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x294)))+(((cj4)*(x290)))+(x292)); -evalcond[2]=((((IkReal(-1.00000000000000))*(x289)*(x292)))+(((cj4)*(x291)))+(((cj4)*(x293)))+(((sj4)*(x294)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(x292)))+(((IkReal(-1.00000000000000))*(x289)*(x293)))+(((IkReal(-1.00000000000000))*(x289)*(x291)))+(((IkReal(-1.00000000000000))*(x290)))+(((cj4)*(x294)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x295=((IkReal(2500.00000000000))*(pp)); -IkReal x296=((IkReal(2100.00000000000))*(cj5)*(npz)); -IkReal x297=((IkReal(1029.00000000000))*(sj3)*(sj4)*(sj5)); -if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x295)))+(((IkReal(-1.00000000000000))*(npy)*(x296)))+(((IkReal(-1.00000000000000))*(npx)*(x297)))+(((IkReal(159.250000000000))*(npy))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x297)))+(((npx)*(x295)))+(((npx)*(x296)))+(((IkReal(-159.250000000000))*(npx))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x295)))+(((IkReal(-1.00000000000000))*(npy)*(x296)))+(((IkReal(-1.00000000000000))*(npx)*(x297)))+(((IkReal(159.250000000000))*(npy)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x297)))+(((npx)*(x295)))+(((npx)*(x296)))+(((IkReal(-159.250000000000))*(npx))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[6]; -IkReal x298=IKcos(j6); -IkReal x299=IKsin(j6); -IkReal x300=((IkReal(1.00000000000000))*(sj4)); -IkReal x301=((cj5)*(npz)); -IkReal x302=((cj4)*(cj5)); -IkReal x303=((IkReal(0.490000000000000))*(sj3)); -IkReal x304=((IkReal(0.490000000000000))*(cj3)); -IkReal x305=((IkReal(0.840000000000000))*(sj5)); -IkReal x306=((npz)*(sj5)); -IkReal x307=((npy)*(x298)); -IkReal x308=((npx)*(x298)); -IkReal x309=((npy)*(x299)); -IkReal x310=((npx)*(x299)); -evalcond[0]=((x307)+(x310)+(((sj4)*(x303)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(x305)*(x309)))+(((x305)*(x308)))+(((IkReal(-0.840000000000000))*(x301)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((sj5)*(x308)))+(((IkReal(-1.00000000000000))*(x301)))+(((IkReal(-1.00000000000000))*(sj5)*(x309)))+(((IkReal(-1.00000000000000))*(x304)))); -evalcond[3]=((((IkReal(-0.420000000000000))*(sj5)))+(((IkReal(-1.00000000000000))*(x302)*(x303)))+(((IkReal(-1.00000000000000))*(x309)))+(x308)+(((IkReal(-1.00000000000000))*(sj5)*(x304)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x300)*(x309)))+(((cj5)*(sj4)*(x308)))+(((cj4)*(x310)))+(((sj4)*(x306)))+(((cj4)*(x307)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x302)*(x309)))+(((IkReal(-1.00000000000000))*(x300)*(x310)))+(((x302)*(x308)))+(((IkReal(-1.00000000000000))*(x300)*(x307)))+(((cj4)*(x306)))+(((IkReal(-1.00000000000000))*(x303)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x311=((IkReal(49.0000000000000))*(npx)); -IkReal x312=((IkReal(49.0000000000000))*(npy)); -IkReal x313=((sj3)*(sj4)*(sj5)); -IkReal x314=((IkReal(100.000000000000))*(cj5)*(npz)); -if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(npy)*(x314)))+(((IkReal(-1.00000000000000))*(x311)*(x313)))+(((IkReal(-1.00000000000000))*(cj3)*(x312)))+(((IkReal(-42.0000000000000))*(npy))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(x312)*(x313)))+(((npx)*(x314)))+(((cj3)*(x311)))+(((IkReal(42.0000000000000))*(npx))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(npy)*(x314)))+(((IkReal(-1.00000000000000))*(x311)*(x313)))+(((IkReal(-1.00000000000000))*(cj3)*(x312)))+(((IkReal(-42.0000000000000))*(npy)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(x312)*(x313)))+(((npx)*(x314)))+(((cj3)*(x311)))+(((IkReal(42.0000000000000))*(npx))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[6]; -IkReal x315=IKcos(j6); -IkReal x316=IKsin(j6); -IkReal x317=((IkReal(1.00000000000000))*(sj4)); -IkReal x318=((cj5)*(npz)); -IkReal x319=((cj4)*(cj5)); -IkReal x320=((IkReal(0.490000000000000))*(sj3)); -IkReal x321=((IkReal(0.490000000000000))*(cj3)); -IkReal x322=((IkReal(0.840000000000000))*(sj5)); -IkReal x323=((npz)*(sj5)); -IkReal x324=((npy)*(x315)); -IkReal x325=((npx)*(x315)); -IkReal x326=((npy)*(x316)); -IkReal x327=((npx)*(x316)); -evalcond[0]=((((sj4)*(x320)))+(x324)+(x327)); -evalcond[1]=((IkReal(0.0637000000000000))+(((x322)*(x325)))+(((IkReal(-0.840000000000000))*(x318)))+(((IkReal(-1.00000000000000))*(x322)*(x326)))+(((IkReal(-1.00000000000000))*(pp)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x321)))+(((sj5)*(x325)))+(((IkReal(-1.00000000000000))*(sj5)*(x326)))+(((IkReal(-1.00000000000000))*(x318)))); -evalcond[3]=((((IkReal(-0.420000000000000))*(sj5)))+(((IkReal(-1.00000000000000))*(x326)))+(x325)+(((IkReal(-1.00000000000000))*(x319)*(x320)))+(((IkReal(-1.00000000000000))*(sj5)*(x321)))); -evalcond[4]=((((cj5)*(sj4)*(x325)))+(((sj4)*(x323)))+(((IkReal(-1.00000000000000))*(cj5)*(x317)*(x326)))+(((cj4)*(x324)))+(((cj4)*(x327)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x317)*(x327)))+(((IkReal(-1.00000000000000))*(x317)*(x324)))+(((IkReal(-1.00000000000000))*(x320)))+(((cj4)*(x323)))+(((IkReal(-1.00000000000000))*(x319)*(x326)))+(((x319)*(x325)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j6array[2], cj6array[2], sj6array[2]; -bool j6valid[2]={false}; -_nj6 = 2; -if( IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x328=((IkReal(1.00000000000000))*(IKatan2(npy, npx))); -if( ((((npx)*(npx))+((npy)*(npy)))) < (IkReal)-0.00001 ) - continue; -if( (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x329=IKasin(((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))); -j6array[0]=((((IkReal(-1.00000000000000))*(x328)))+(((IkReal(-1.00000000000000))*(x329)))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -j6array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x328)))+(x329)); -sj6array[1]=IKsin(j6array[1]); -cj6array[1]=IKcos(j6array[1]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -if( j6array[1] > IKPI ) -{ - j6array[1]-=IK2PI; -} -else if( j6array[1] < -IKPI ) -{ j6array[1]+=IK2PI; -} -j6valid[1] = true; -for(int ij6 = 0; ij6 < 2; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 2; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x330=(npx)*(npx); -IkReal x331=(cj4)*(cj4); -IkReal x332=(sj4)*(sj4); -IkReal x333=IKcos(j6); -IkReal x334=(npy)*(npy); -IkReal x335=IKsin(j6); -IkReal x336=((npx)*(npy)); -IkReal x337=((IkReal(0.490000000000000))*(sj3)*(sj4)); -IkReal x338=((IkReal(1.00000000000000))*(x334)); -evalcond[0]=((((x333)*(((((x331)*(x336)))+(((x332)*(x336)))))))+(((x335)*(((((x330)*(x331)))+(((x330)*(x332)))))))+(((npx)*(x337)))); -evalcond[1]=((((x335)*(((((IkReal(-1.00000000000000))*(x332)*(x336)))+(((IkReal(-1.00000000000000))*(x331)*(x336)))))))+(((x333)*(((((IkReal(-1.00000000000000))*(x331)*(x338)))+(((IkReal(-1.00000000000000))*(x332)*(x338)))))))+(((IkReal(-1.00000000000000))*(npy)*(x337)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst10; -IkReal x339=((cj6)*(npx)); -IkReal x340=((IkReal(49.0000000000000))*(cj3)); -IkReal x341=((npy)*(sj6)); -gconst10=IKsign(((((IkReal(-42.0000000000000))*(x339)))+(((IkReal(49.0000000000000))*(cj4)*(npz)*(sj3)))+(((x340)*(x341)))+(((IkReal(42.0000000000000))*(x341)))+(((IkReal(-1.00000000000000))*(x339)*(x340))))); -IkReal x342=((npy)*(sj6)); -IkReal x343=((cj6)*(npx)); -IkReal x344=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(x342)+(((IkReal(-1.00000000000000))*(x343)*(x344)))+(((IkReal(-1.00000000000000))*(x343)))+(((x342)*(x344)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst11; -IkReal x345=((npy)*(sj6)); -IkReal x346=((IkReal(1029.00000000000))*(cj3)); -IkReal x347=((cj6)*(npx)); -gconst11=IKsign(((((x345)*(x346)))+(((IkReal(1029.00000000000))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x346)*(x347)))+(((IkReal(-882.000000000000))*(x347)))+(((IkReal(882.000000000000))*(x345))))); -IkReal x348=((npy)*(sj6)); -IkReal x349=((cj6)*(npx)); -IkReal x350=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x349)*(x350)))+(x348)+(((x348)*(x350)))+(((IkReal(-1.00000000000000))*(x349)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x351=((cj4)*(sj3)); -IkReal x352=((IkReal(1225.00000000000))*(pp)); -IkReal x353=((IkReal(2100.00000000000))*(npz)); -if( IKabs(((gconst11)*(((IkReal(66.8850000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x352)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(78.0325000000000))*(cj3)))+(((npz)*(x353))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((IkReal(78.0325000000000))*(x351)))+(((cj6)*(npx)*(x353)))+(((IkReal(-1.00000000000000))*(x351)*(x352)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x353))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst11)*(((IkReal(66.8850000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x352)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(78.0325000000000))*(cj3)))+(((npz)*(x353)))))), ((gconst11)*(((((IkReal(78.0325000000000))*(x351)))+(((cj6)*(npx)*(x353)))+(((IkReal(-1.00000000000000))*(x351)*(x352)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(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[6]; -IkReal x354=IKcos(j5); -IkReal x355=IKsin(j5); -IkReal x356=((IkReal(1.00000000000000))*(npy)); -IkReal x357=((cj6)*(sj4)); -IkReal x358=((IkReal(0.490000000000000))*(sj3)); -IkReal x359=((sj4)*(sj6)); -IkReal x360=((IkReal(1.00000000000000))*(npz)); -IkReal x361=((cj6)*(npx)); -IkReal x362=((IkReal(0.490000000000000))*(cj3)); -IkReal x363=((npz)*(x355)); -IkReal x364=((cj4)*(x354)); -IkReal x365=((sj6)*(x355)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x354)*(x362)))+(((IkReal(-0.420000000000000))*(x354)))+(((IkReal(-1.00000000000000))*(x360)))+(((cj4)*(x355)*(x358)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(-0.840000000000000))*(npz)*(x354)))+(((IkReal(0.840000000000000))*(x355)*(x361)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x365)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x354)*(x360)))+(((IkReal(-1.00000000000000))*(x362)))+(((x355)*(x361)))+(((IkReal(-1.00000000000000))*(x356)*(x365)))); -evalcond[3]=((((IkReal(-0.420000000000000))*(x355)))+(((IkReal(-1.00000000000000))*(x358)*(x364)))+(((IkReal(-1.00000000000000))*(x355)*(x362)))+(x361)+(((IkReal(-1.00000000000000))*(sj6)*(x356)))); -evalcond[4]=((((sj4)*(x363)))+(((npx)*(x354)*(x357)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))+(((IkReal(-1.00000000000000))*(x354)*(x356)*(x359)))); -evalcond[5]=((((x361)*(x364)))+(((IkReal(-1.00000000000000))*(x358)))+(((IkReal(-1.00000000000000))*(x356)*(x357)))+(((cj4)*(x363)))+(((IkReal(-1.00000000000000))*(npx)*(x359)))+(((IkReal(-1.00000000000000))*(sj6)*(x356)*(x364)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x366=((cj4)*(sj3)); -IkReal x367=((IkReal(100.000000000000))*(npz)); -if( IKabs(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x367)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-20.5800000000000))*(x366)))+(((cj6)*(npx)*(x367)))+(((IkReal(-24.0100000000000))*(cj3)*(x366)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x367))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x367)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3))))))), ((gconst10)*(((((IkReal(-20.5800000000000))*(x366)))+(((cj6)*(npx)*(x367)))+(((IkReal(-24.0100000000000))*(cj3)*(x366)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x367))))))); -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 x368=IKcos(j5); -IkReal x369=IKsin(j5); -IkReal x370=((IkReal(1.00000000000000))*(npy)); -IkReal x371=((cj6)*(sj4)); -IkReal x372=((IkReal(0.490000000000000))*(sj3)); -IkReal x373=((sj4)*(sj6)); -IkReal x374=((IkReal(1.00000000000000))*(npz)); -IkReal x375=((cj6)*(npx)); -IkReal x376=((IkReal(0.490000000000000))*(cj3)); -IkReal x377=((npz)*(x369)); -IkReal x378=((cj4)*(x368)); -IkReal x379=((sj6)*(x369)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x368)*(x376)))+(((cj4)*(x369)*(x372)))+(((IkReal(-1.00000000000000))*(x374)))+(((IkReal(-0.420000000000000))*(x368)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(x369)*(x375)))+(((IkReal(-0.840000000000000))*(npz)*(x368)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x379)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x368)*(x374)))+(((IkReal(-1.00000000000000))*(x370)*(x379)))+(((x369)*(x375)))+(((IkReal(-1.00000000000000))*(x376)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x372)*(x378)))+(x375)+(((IkReal(-0.420000000000000))*(x369)))+(((IkReal(-1.00000000000000))*(sj6)*(x370)))+(((IkReal(-1.00000000000000))*(x369)*(x376)))); -evalcond[4]=((((sj4)*(x377)))+(((IkReal(-1.00000000000000))*(x368)*(x370)*(x373)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))+(((npx)*(x368)*(x371)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(npx)*(x373)))+(((x375)*(x378)))+(((IkReal(-1.00000000000000))*(x370)*(x371)))+(((IkReal(-1.00000000000000))*(x372)))+(((cj4)*(x377)))+(((IkReal(-1.00000000000000))*(sj6)*(x370)*(x378)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j6array[2], cj6array[2], sj6array[2]; -bool j6valid[2]={false}; -_nj6 = 2; -if( IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x380=((IkReal(1.00000000000000))*(IKatan2(npy, npx))); -if( ((((npx)*(npx))+((npy)*(npy)))) < (IkReal)-0.00001 ) - continue; -if( (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x381=IKasin(((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))); -j6array[0]=((((IkReal(-1.00000000000000))*(x381)))+(((IkReal(-1.00000000000000))*(x380)))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -j6array[1]=((IkReal(3.14159265358979))+(x381)+(((IkReal(-1.00000000000000))*(x380)))); -sj6array[1]=IKsin(j6array[1]); -cj6array[1]=IKcos(j6array[1]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -if( j6array[1] > IKPI ) -{ - j6array[1]-=IK2PI; -} -else if( j6array[1] < -IKPI ) -{ j6array[1]+=IK2PI; -} -j6valid[1] = true; -for(int ij6 = 0; ij6 < 2; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 2; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; - -{ -IkReal dummyeval[1]; -IkReal gconst10; -IkReal x382=((cj6)*(npx)); -IkReal x383=((IkReal(49.0000000000000))*(cj3)); -IkReal x384=((npy)*(sj6)); -gconst10=IKsign(((((IkReal(-42.0000000000000))*(x382)))+(((IkReal(42.0000000000000))*(x384)))+(((x383)*(x384)))+(((IkReal(49.0000000000000))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x382)*(x383))))); -IkReal x385=((npy)*(sj6)); -IkReal x386=((cj6)*(npx)); -IkReal x387=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(x385)+(((IkReal(-1.00000000000000))*(x386)))+(((IkReal(-1.00000000000000))*(x386)*(x387)))+(((x385)*(x387)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst11; -IkReal x388=((npy)*(sj6)); -IkReal x389=((IkReal(1029.00000000000))*(cj3)); -IkReal x390=((cj6)*(npx)); -gconst11=IKsign(((((IkReal(-1.00000000000000))*(x389)*(x390)))+(((IkReal(-882.000000000000))*(x390)))+(((x388)*(x389)))+(((IkReal(1029.00000000000))*(cj4)*(npz)*(sj3)))+(((IkReal(882.000000000000))*(x388))))); -IkReal x391=((npy)*(sj6)); -IkReal x392=((cj6)*(npx)); -IkReal x393=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x392)))+(x391)+(((x391)*(x393)))+(((IkReal(-1.00000000000000))*(x392)*(x393)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x394=((cj4)*(sj3)); -IkReal x395=((IkReal(1225.00000000000))*(pp)); -IkReal x396=((IkReal(2100.00000000000))*(npz)); -if( IKabs(((gconst11)*(((IkReal(66.8850000000000))+(((npz)*(x396)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj3)*(x395)))+(((IkReal(78.0325000000000))*(cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj6)*(npx)*(x396)))+(((IkReal(-1.00000000000000))*(x394)*(x395)))+(((IkReal(78.0325000000000))*(x394)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x396))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst11)*(((IkReal(66.8850000000000))+(((npz)*(x396)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj3)*(x395)))+(((IkReal(78.0325000000000))*(cj3)))))), ((gconst11)*(((((cj6)*(npx)*(x396)))+(((IkReal(-1.00000000000000))*(x394)*(x395)))+(((IkReal(78.0325000000000))*(x394)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x396))))))); -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 x397=IKcos(j5); -IkReal x398=IKsin(j5); -IkReal x399=((IkReal(1.00000000000000))*(npy)); -IkReal x400=((cj6)*(sj4)); -IkReal x401=((IkReal(0.490000000000000))*(sj3)); -IkReal x402=((sj4)*(sj6)); -IkReal x403=((IkReal(1.00000000000000))*(npz)); -IkReal x404=((cj6)*(npx)); -IkReal x405=((IkReal(0.490000000000000))*(cj3)); -IkReal x406=((npz)*(x398)); -IkReal x407=((cj4)*(x397)); -IkReal x408=((sj6)*(x398)); -evalcond[0]=((((cj4)*(x398)*(x401)))+(((IkReal(-1.00000000000000))*(x397)*(x405)))+(((IkReal(-0.420000000000000))*(x397)))+(((IkReal(-1.00000000000000))*(x403)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(x398)*(x404)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x408)))+(((IkReal(-0.840000000000000))*(npz)*(x397)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x399)*(x408)))+(((IkReal(-1.00000000000000))*(x397)*(x403)))+(((x398)*(x404)))+(((IkReal(-1.00000000000000))*(x405)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x401)*(x407)))+(((IkReal(-1.00000000000000))*(sj6)*(x399)))+(x404)+(((IkReal(-1.00000000000000))*(x398)*(x405)))+(((IkReal(-0.420000000000000))*(x398)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x397)*(x399)*(x402)))+(((npx)*(x397)*(x400)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))+(((sj4)*(x406)))); -evalcond[5]=((((x404)*(x407)))+(((IkReal(-1.00000000000000))*(sj6)*(x399)*(x407)))+(((cj4)*(x406)))+(((IkReal(-1.00000000000000))*(x399)*(x400)))+(((IkReal(-1.00000000000000))*(npx)*(x402)))+(((IkReal(-1.00000000000000))*(x401)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x409=((cj4)*(sj3)); -IkReal x410=((IkReal(100.000000000000))*(npz)); -if( IKabs(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x410)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(npy)*(sj6)*(x410)))+(((cj6)*(npx)*(x410)))+(((IkReal(-24.0100000000000))*(cj3)*(x409)))+(((IkReal(-20.5800000000000))*(x409))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x410)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3))))))), ((gconst10)*(((((IkReal(-1.00000000000000))*(npy)*(sj6)*(x410)))+(((cj6)*(npx)*(x410)))+(((IkReal(-24.0100000000000))*(cj3)*(x409)))+(((IkReal(-20.5800000000000))*(x409))))))); -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 x411=IKcos(j5); -IkReal x412=IKsin(j5); -IkReal x413=((IkReal(1.00000000000000))*(npy)); -IkReal x414=((cj6)*(sj4)); -IkReal x415=((IkReal(0.490000000000000))*(sj3)); -IkReal x416=((sj4)*(sj6)); -IkReal x417=((IkReal(1.00000000000000))*(npz)); -IkReal x418=((cj6)*(npx)); -IkReal x419=((IkReal(0.490000000000000))*(cj3)); -IkReal x420=((npz)*(x412)); -IkReal x421=((cj4)*(x411)); -IkReal x422=((sj6)*(x412)); -evalcond[0]=((((cj4)*(x412)*(x415)))+(((IkReal(-0.420000000000000))*(x411)))+(((IkReal(-1.00000000000000))*(x411)*(x419)))+(((IkReal(-1.00000000000000))*(x417)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(x412)*(x418)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x422)))+(((IkReal(-0.840000000000000))*(npz)*(x411)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x411)*(x417)))+(((x412)*(x418)))+(((IkReal(-1.00000000000000))*(x413)*(x422)))+(((IkReal(-1.00000000000000))*(x419)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(sj6)*(x413)))+(((IkReal(-1.00000000000000))*(x412)*(x419)))+(((IkReal(-0.420000000000000))*(x412)))+(x418)+(((IkReal(-1.00000000000000))*(x415)*(x421)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x411)*(x413)*(x416)))+(((npx)*(x411)*(x414)))+(((sj4)*(x420)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(sj6)*(x413)*(x421)))+(((x418)*(x421)))+(((cj4)*(x420)))+(((IkReal(-1.00000000000000))*(x413)*(x414)))+(((IkReal(-1.00000000000000))*(npx)*(x416)))+(((IkReal(-1.00000000000000))*(x415)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} - -} - -} -} -} -} -return solutions.GetNumSolutions()>0; -} -inline void rotationfunction0(IkSolutionListBase& solutions) { -for(int rotationiter = 0; rotationiter < 1; ++rotationiter) { -IkReal x78=((cj6)*(sj4)); -IkReal x79=((IkReal(1.00000000000000))*(sj5)); -IkReal x80=((cj4)*(cj5)); -IkReal x81=((IkReal(1.00000000000000))*(sj3)); -IkReal x82=((cj3)*(sj5)); -IkReal x83=((IkReal(1.00000000000000))*(sj6)); -IkReal x84=((sj4)*(sj6)); -IkReal x85=((IkReal(1.00000000000000))*(cj3)); -IkReal x86=((IkReal(-1.00000000000000))*(cj3)); -IkReal x87=((((cj3)*(x80)))+(((IkReal(-1.00000000000000))*(sj3)*(x79)))); -IkReal x88=((((cj5)*(sj3)))+(((cj4)*(x82)))); -IkReal x89=((((cj5)*(x84)))+(((IkReal(-1.00000000000000))*(cj4)*(cj6)))); -IkReal x90=((x82)+(((sj3)*(x80)))); -IkReal x91=((((cj4)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x85)))); -IkReal x92=((cj6)*(x87)); -IkReal x93=((((IkReal(-1.00000000000000))*(cj4)*(x83)))+(((IkReal(-1.00000000000000))*(cj5)*(x78)))); -IkReal x94=((((IkReal(-1.00000000000000))*(cj3)*(sj4)*(x83)))+(x92)); -IkReal x95=((((IkReal(-1.00000000000000))*(x81)*(x84)))+(((cj6)*(x90)))); -IkReal x96=((((IkReal(-1.00000000000000))*(x83)*(x90)))+(((IkReal(-1.00000000000000))*(x78)*(x81)))); -IkReal x97=((((x78)*(x86)))+(((IkReal(-1.00000000000000))*(sj6)*(x87)))); -new_r00=((((r00)*(((((x84)*(x86)))+(x92)))))+(((r01)*(((((IkReal(-1.00000000000000))*(x83)*(x87)))+(((IkReal(-1.00000000000000))*(x78)*(x85)))))))+(((r02)*(x88)))); -new_r01=((((r12)*(x88)))+(((r11)*(x97)))+(((r10)*(x94)))); -new_r02=((((r21)*(x97)))+(((r20)*(x94)))+(((r22)*(x88)))); -new_r10=((((r00)*(x93)))+(((r01)*(x89)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x79)))); -new_r11=((((r11)*(x89)))+(((r10)*(x93)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x79)))); -new_r12=((((r21)*(x89)))+(((IkReal(-1.00000000000000))*(r22)*(sj4)*(x79)))+(((r20)*(x93)))); -new_r20=((((r01)*(x96)))+(((r00)*(x95)))+(((r02)*(x91)))); -new_r21=((((r12)*(x91)))+(((r11)*(x96)))+(((r10)*(x95)))); -new_r22=((((r21)*(x96)))+(((r20)*(x95)))+(((r22)*(x91)))); -{ -IkReal j1array[2], cj1array[2], sj1array[2]; -bool j1valid[2]={false}; -_nj1 = 2; -cj1array[0]=new_r22; -if( cj1array[0] >= -1-IKFAST_SINCOS_THRESH && cj1array[0] <= 1+IKFAST_SINCOS_THRESH ) -{ - j1valid[0] = j1valid[1] = true; - j1array[0] = IKacos(cj1array[0]); - sj1array[0] = IKsin(j1array[0]); - cj1array[1] = cj1array[0]; - j1array[1] = -j1array[0]; - sj1array[1] = -sj1array[0]; -} -else if( isnan(cj1array[0]) ) -{ - // probably any value will work - j1valid[0] = true; - cj1array[0] = 1; sj1array[0] = 0; j1array[0] = 0; -} -for(int ij1 = 0; ij1 < 2; ++ij1) -{ -if( !j1valid[ij1] ) -{ - continue; -} -_ij1[0] = ij1; _ij1[1] = -1; -for(int iij1 = ij1+1; iij1 < 2; ++iij1) -{ -if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) -{ - j1valid[iij1]=false; _ij1[1] = iij1; break; -} -} -j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; - -{ -IkReal dummyeval[1]; -IkReal gconst12; -gconst12=IKsign(sj1); -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst13; -gconst13=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst14; -gconst14=IKsign(((((cj1)*((new_r12)*(new_r12))))+(((cj1)*((new_r02)*(new_r02)))))); -dummyeval[0]=((((cj1)*((new_r12)*(new_r12))))+(((cj1)*((new_r02)*(new_r02))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[7]; -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=new_r22; -evalcond[2]=new_r22; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(new_r21, new_r20); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[2]; -evalcond[0]=((((IkReal(-1.00000000000000))*(IKcos(j0))))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(IKsin(j0))))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst21; -gconst21=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst20; -gconst20=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x98=((gconst20)*(sj0)); -if( IKabs(((new_r12)*(x98))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x98))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x98)), ((IkReal(-1.00000000000000))*(new_r02)*(x98))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x99=IKsin(j2); -IkReal x100=IKcos(j2); -IkReal x101=((IkReal(1.00000000000000))*(x99)); -evalcond[0]=((((new_r02)*(x99)))+(((new_r12)*(x100)))); -evalcond[1]=((((new_r10)*(x100)))+(sj0)+(((new_r00)*(x99)))); -evalcond[2]=((IkReal(1.00000000000000))+(((new_r02)*(x100)))+(((IkReal(-1.00000000000000))*(new_r12)*(x101)))); -evalcond[3]=((((new_r01)*(x99)))+(((IkReal(-1.00000000000000))*(cj0)))+(((new_r11)*(x100)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x101)))+(((new_r00)*(x100)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x101)))+(((new_r01)*(x100)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -if( IKabs(((gconst21)*(new_r12))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst21)*(new_r02))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst21)*(new_r12)), ((IkReal(-1.00000000000000))*(gconst21)*(new_r02))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x102=IKsin(j2); -IkReal x103=IKcos(j2); -IkReal x104=((IkReal(1.00000000000000))*(x102)); -evalcond[0]=((((new_r02)*(x102)))+(((new_r12)*(x103)))); -evalcond[1]=((((new_r10)*(x103)))+(sj0)+(((new_r00)*(x102)))); -evalcond[2]=((IkReal(1.00000000000000))+(((new_r02)*(x103)))+(((IkReal(-1.00000000000000))*(new_r12)*(x104)))); -evalcond[3]=((((new_r01)*(x102)))+(((IkReal(-1.00000000000000))*(cj0)))+(((new_r11)*(x103)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x104)))+(((new_r00)*(x103)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x104)))+(((new_r01)*(x103)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} else -{ -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=new_r22; -evalcond[2]=((IkReal(-1.00000000000000))*(new_r22)); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((IkReal(-1.00000000000000))*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)), ((IkReal(-1.00000000000000))*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[2]; -evalcond[0]=((IKcos(j0))+(new_r20)); -evalcond[1]=((IKsin(j0))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst24; -gconst24=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst25; -gconst25=IKsign(((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -if( IKabs(((gconst25)*(new_r12))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst25)*(new_r02))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst25)*(new_r12)), ((IkReal(-1.00000000000000))*(gconst25)*(new_r02))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x105=IKsin(j2); -IkReal x106=IKcos(j2); -IkReal x107=((IkReal(1.00000000000000))*(x105)); -evalcond[0]=((((new_r02)*(x105)))+(((new_r12)*(x106)))); -evalcond[1]=((((new_r10)*(x106)))+(sj0)+(((new_r00)*(x105)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((new_r02)*(x106)))+(((IkReal(-1.00000000000000))*(new_r12)*(x107)))); -evalcond[3]=((((new_r01)*(x105)))+(((IkReal(-1.00000000000000))*(cj0)))+(((new_r11)*(x106)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x107)))+(((new_r00)*(x106)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x107)))+(((new_r01)*(x106)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x108=((gconst24)*(sj0)); -if( IKabs(((new_r12)*(x108))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x108))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x108)), ((IkReal(-1.00000000000000))*(new_r02)*(x108))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x109=IKsin(j2); -IkReal x110=IKcos(j2); -IkReal x111=((IkReal(1.00000000000000))*(x109)); -evalcond[0]=((((new_r02)*(x109)))+(((new_r12)*(x110)))); -evalcond[1]=((((new_r10)*(x110)))+(sj0)+(((new_r00)*(x109)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((new_r02)*(x110)))+(((IkReal(-1.00000000000000))*(new_r12)*(x111)))); -evalcond[3]=((((new_r11)*(x110)))+(((new_r01)*(x109)))+(((IkReal(-1.00000000000000))*(cj0)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x111)))+(((new_r00)*(x110)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x111)))+(((new_r01)*(x110)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} else -{ -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[5]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[6]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 ) -{ -{ -IkReal j2array[2], cj2array[2], sj2array[2]; -bool j2valid[2]={false}; -_nj2 = 2; -if( IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x112=IKatan2(new_r12, new_r02); -j2array[0]=((IkReal(-1.00000000000000))*(x112)); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -j2array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x112)))); -sj2array[1]=IKsin(j2array[1]); -cj2array[1]=IKcos(j2array[1]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -if( j2array[1] > IKPI ) -{ - j2array[1]-=IK2PI; -} -else if( j2array[1] < -IKPI ) -{ j2array[1]+=IK2PI; -} -j2valid[1] = true; -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 evalcond[1]; -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r12)*(IKsin(j2))))+(((new_r02)*(IKcos(j2))))); -if( IKabs(evalcond[0]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x113=IKsin(j0); -IkReal x114=((IkReal(1.00000000000000))*(sj2)); -IkReal x115=((IkReal(1.00000000000000))*(IKcos(j0))); -evalcond[0]=((((new_r00)*(sj2)))+(x113)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x115)))+(((cj2)*(new_r11)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x114)))+(((IkReal(-1.00000000000000))*(x115)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x114)))+(((cj2)*(new_r01)))+(((IkReal(-1.00000000000000))*(x113)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} -} -} - -} else -{ -IkReal x116=((IkReal(1.00000000000000))+(new_r22)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=x116; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=new_r20; -evalcond[5]=new_r21; -evalcond[6]=x116; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 ) -{ -{ -IkReal j2array[2], cj2array[2], sj2array[2]; -bool j2valid[2]={false}; -_nj2 = 2; -if( IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x117=IKatan2(new_r12, new_r02); -j2array[0]=((IkReal(-1.00000000000000))*(x117)); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -j2array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x117)))); -sj2array[1]=IKsin(j2array[1]); -cj2array[1]=IKcos(j2array[1]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -if( j2array[1] > IKPI ) -{ - j2array[1]-=IK2PI; -} -else if( j2array[1] < -IKPI ) -{ j2array[1]+=IK2PI; -} -j2valid[1] = true; -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 evalcond[1]; -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r12)*(IKsin(j2))))+(((new_r02)*(IKcos(j2))))); -if( IKabs(evalcond[0]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x118=IKcos(j0); -IkReal x119=IKsin(j0); -IkReal x120=((IkReal(1.00000000000000))*(sj2)); -evalcond[0]=((((new_r00)*(sj2)))+(x119)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((cj2)*(new_r11)))+(((IkReal(-1.00000000000000))*(x118)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x120)))+(x118)+(((cj2)*(new_r00)))); -evalcond[3]=((x119)+(((cj2)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(x120)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} -} -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} -} -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x121=((gconst14)*(new_r22)*(sj1)); -if( IKabs(((new_r12)*(x121))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x121))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x121)), ((IkReal(-1.00000000000000))*(new_r02)*(x121))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x122=IKcos(j2); -IkReal x123=IKsin(j2); -IkReal x124=((IkReal(1.00000000000000))*(cj1)); -IkReal x125=((sj1)*(x122)); -IkReal x126=((new_r12)*(x123)); -IkReal x127=((new_r02)*(x122)); -IkReal x128=((IkReal(1.00000000000000))*(sj1)*(x123)); -evalcond[0]=((((new_r02)*(x123)))+(((new_r12)*(x122)))); -evalcond[1]=((sj1)+(x127)+(((IkReal(-1.00000000000000))*(x126)))); -evalcond[2]=((((cj1)*(x127)))+(((IkReal(-1.00000000000000))*(x124)*(x126)))+(((new_r22)*(sj1)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r10)*(x128)))+(((new_r00)*(x125)))+(((IkReal(-1.00000000000000))*(new_r20)*(x124)))); -evalcond[4]=((((new_r01)*(x125)))+(((IkReal(-1.00000000000000))*(new_r21)*(x124)))+(((IkReal(-1.00000000000000))*(new_r11)*(x128)))); -evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x126)))+(((new_r02)*(x125)))+(((IkReal(-1.00000000000000))*(new_r22)*(x124)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst15; -gconst15=IKsign(sj1); -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x129=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -IkReal x130=((((cj2)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj2)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x129; -evalcond[5]=x129; -evalcond[6]=x130; -evalcond[7]=x130; -evalcond[8]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[9]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x131=IKsin(j0); -IkReal x132=((IkReal(1.00000000000000))*(sj2)); -IkReal x133=((IkReal(1.00000000000000))*(IKcos(j0))); -evalcond[0]=((((new_r00)*(sj2)))+(x131)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x133)))+(((cj2)*(new_r11)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x132)))+(((IkReal(-1.00000000000000))*(x133)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x132)))+(((cj2)*(new_r01)))+(((IkReal(-1.00000000000000))*(x131)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -IkReal x134=((IkReal(1.00000000000000))+(new_r22)); -IkReal x135=((new_r12)*(sj2)); -IkReal x136=((cj2)*(new_r02)); -IkReal x137=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=x134; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x137; -evalcond[5]=x137; -evalcond[6]=((x136)+(((IkReal(-1.00000000000000))*(x135)))); -evalcond[7]=((x135)+(((IkReal(-1.00000000000000))*(x136)))); -evalcond[8]=new_r20; -evalcond[9]=new_r21; -evalcond[10]=x134; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x138=IKcos(j0); -IkReal x139=IKsin(j0); -IkReal x140=((IkReal(1.00000000000000))*(sj2)); -evalcond[0]=((((new_r00)*(sj2)))+(x139)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((cj2)*(new_r11)))+(((IkReal(-1.00000000000000))*(x138)))); -evalcond[2]=((x138)+(((IkReal(-1.00000000000000))*(new_r10)*(x140)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x140)))+(x139)+(((cj2)*(new_r01)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x141=IKsin(j0); -IkReal x142=IKcos(j0); -IkReal x143=((cj2)*(new_r01)); -IkReal x144=((IkReal(1.00000000000000))*(cj1)); -IkReal x145=((IkReal(1.00000000000000))*(sj1)); -IkReal x146=((new_r10)*(sj2)); -IkReal x147=((new_r11)*(sj2)); -IkReal x148=((cj2)*(new_r00)); -IkReal x149=((IkReal(1.00000000000000))*(x142)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x142)*(x145)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x141)*(x145)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x141)+(((cj2)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x149)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x142)*(x144)))+(x148)+(((IkReal(-1.00000000000000))*(x146)))); -evalcond[5]=((x143)+(((IkReal(-1.00000000000000))*(x147)))+(((IkReal(-1.00000000000000))*(x141)*(x144)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x149)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x144)*(x146)))+(((cj1)*(x148)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x144)*(x147)))+(((IkReal(-1.00000000000000))*(x141)))+(((new_r21)*(sj1)))+(((cj1)*(x143)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x150=IKsin(j0); -IkReal x151=IKcos(j0); -IkReal x152=((cj2)*(new_r01)); -IkReal x153=((IkReal(1.00000000000000))*(cj1)); -IkReal x154=((IkReal(1.00000000000000))*(sj1)); -IkReal x155=((new_r10)*(sj2)); -IkReal x156=((new_r11)*(sj2)); -IkReal x157=((cj2)*(new_r00)); -IkReal x158=((IkReal(1.00000000000000))*(x151)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x151)*(x154)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x150)*(x154)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x150)+(((cj2)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x158)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x151)*(x153)))+(x157)+(((IkReal(-1.00000000000000))*(x155)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x150)*(x153)))+(x152)+(((IkReal(-1.00000000000000))*(x156)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x158)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x153)*(x155)))+(((cj1)*(x157)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x153)*(x156)))+(((IkReal(-1.00000000000000))*(x150)))+(((new_r21)*(sj1)))+(((cj1)*(x152)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((gconst15)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j0array[0]=IKatan2(((gconst15)*(new_r21)), ((gconst15)*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x159=IKsin(j0); -IkReal x160=IKcos(j0); -IkReal x161=((cj2)*(new_r01)); -IkReal x162=((IkReal(1.00000000000000))*(cj1)); -IkReal x163=((IkReal(1.00000000000000))*(sj1)); -IkReal x164=((new_r10)*(sj2)); -IkReal x165=((new_r11)*(sj2)); -IkReal x166=((cj2)*(new_r00)); -IkReal x167=((IkReal(1.00000000000000))*(x160)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x160)*(x163)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x159)*(x163)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x159)+(((cj2)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x167)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x160)*(x162)))+(x166)+(((IkReal(-1.00000000000000))*(x164)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x159)*(x162)))+(x161)+(((IkReal(-1.00000000000000))*(x165)))); -evalcond[6]=((((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x167)))+(((IkReal(-1.00000000000000))*(x162)*(x164)))+(((cj1)*(x166)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x162)*(x165)))+(((IkReal(-1.00000000000000))*(x159)))+(((cj1)*(x161)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x168=((gconst13)*(sj1)); -if( IKabs(((new_r12)*(x168))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x168))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x168)), ((IkReal(-1.00000000000000))*(new_r02)*(x168))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x169=IKcos(j2); -IkReal x170=IKsin(j2); -IkReal x171=((IkReal(1.00000000000000))*(cj1)); -IkReal x172=((sj1)*(x169)); -IkReal x173=((new_r12)*(x170)); -IkReal x174=((new_r02)*(x169)); -IkReal x175=((IkReal(1.00000000000000))*(sj1)*(x170)); -evalcond[0]=((((new_r02)*(x170)))+(((new_r12)*(x169)))); -evalcond[1]=((sj1)+(((IkReal(-1.00000000000000))*(x173)))+(x174)); -evalcond[2]=((((new_r22)*(sj1)))+(((IkReal(-1.00000000000000))*(x171)*(x173)))+(((cj1)*(x174)))); -evalcond[3]=((((new_r00)*(x172)))+(((IkReal(-1.00000000000000))*(new_r20)*(x171)))+(((IkReal(-1.00000000000000))*(new_r10)*(x175)))); -evalcond[4]=((((new_r01)*(x172)))+(((IkReal(-1.00000000000000))*(new_r21)*(x171)))+(((IkReal(-1.00000000000000))*(new_r11)*(x175)))); -evalcond[5]=((IkReal(1.00000000000000))+(((new_r02)*(x172)))+(((IkReal(-1.00000000000000))*(sj1)*(x173)))+(((IkReal(-1.00000000000000))*(new_r22)*(x171)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst15; -gconst15=IKsign(sj1); -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x176=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -IkReal x177=((((cj2)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj2)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x176; -evalcond[5]=x176; -evalcond[6]=x177; -evalcond[7]=x177; -evalcond[8]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[9]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x178=IKsin(j0); -IkReal x179=((IkReal(1.00000000000000))*(sj2)); -IkReal x180=((IkReal(1.00000000000000))*(IKcos(j0))); -evalcond[0]=((((new_r00)*(sj2)))+(x178)+(((cj2)*(new_r10)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x180)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x179)))+(((IkReal(-1.00000000000000))*(x180)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x178)))+(((IkReal(-1.00000000000000))*(new_r11)*(x179)))+(((cj2)*(new_r01)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -IkReal x181=((IkReal(1.00000000000000))+(new_r22)); -IkReal x182=((new_r12)*(sj2)); -IkReal x183=((cj2)*(new_r02)); -IkReal x184=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=x181; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x184; -evalcond[5]=x184; -evalcond[6]=((((IkReal(-1.00000000000000))*(x182)))+(x183)); -evalcond[7]=((((IkReal(-1.00000000000000))*(x183)))+(x182)); -evalcond[8]=new_r20; -evalcond[9]=new_r21; -evalcond[10]=x181; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x185=IKcos(j0); -IkReal x186=IKsin(j0); -IkReal x187=((IkReal(1.00000000000000))*(sj2)); -evalcond[0]=((((new_r00)*(sj2)))+(x186)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x185)))+(((cj2)*(new_r11)))); -evalcond[2]=((x185)+(((IkReal(-1.00000000000000))*(new_r10)*(x187)))+(((cj2)*(new_r00)))); -evalcond[3]=((x186)+(((IkReal(-1.00000000000000))*(new_r11)*(x187)))+(((cj2)*(new_r01)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x188=IKsin(j0); -IkReal x189=IKcos(j0); -IkReal x190=((cj2)*(new_r01)); -IkReal x191=((IkReal(1.00000000000000))*(cj1)); -IkReal x192=((IkReal(1.00000000000000))*(sj1)); -IkReal x193=((new_r10)*(sj2)); -IkReal x194=((new_r11)*(sj2)); -IkReal x195=((cj2)*(new_r00)); -IkReal x196=((IkReal(1.00000000000000))*(x189)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x189)*(x192)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x188)*(x192)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x188)+(((cj2)*(new_r10)))); -evalcond[3]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x196)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x189)*(x191)))+(((IkReal(-1.00000000000000))*(x193)))+(x195)); -evalcond[5]=((((IkReal(-1.00000000000000))*(x188)*(x191)))+(((IkReal(-1.00000000000000))*(x194)))+(x190)); -evalcond[6]=((((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x191)*(x193)))+(((IkReal(-1.00000000000000))*(x196)))+(((cj1)*(x195)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x191)*(x194)))+(((IkReal(-1.00000000000000))*(x188)))+(((cj1)*(x190)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x197=IKsin(j0); -IkReal x198=IKcos(j0); -IkReal x199=((cj2)*(new_r01)); -IkReal x200=((IkReal(1.00000000000000))*(cj1)); -IkReal x201=((IkReal(1.00000000000000))*(sj1)); -IkReal x202=((new_r10)*(sj2)); -IkReal x203=((new_r11)*(sj2)); -IkReal x204=((cj2)*(new_r00)); -IkReal x205=((IkReal(1.00000000000000))*(x198)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x198)*(x201)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x197)*(x201)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x197)+(((cj2)*(new_r10)))); -evalcond[3]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x205)))+(((cj2)*(new_r11)))); -evalcond[4]=((x204)+(((IkReal(-1.00000000000000))*(x198)*(x200)))+(((IkReal(-1.00000000000000))*(x202)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x197)*(x200)))+(x199)+(((IkReal(-1.00000000000000))*(x203)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x200)*(x202)))+(((cj1)*(x204)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x205)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x200)*(x203)))+(((IkReal(-1.00000000000000))*(x197)))+(((cj1)*(x199)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((gconst15)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j0array[0]=IKatan2(((gconst15)*(new_r21)), ((gconst15)*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x206=IKsin(j0); -IkReal x207=IKcos(j0); -IkReal x208=((cj2)*(new_r01)); -IkReal x209=((IkReal(1.00000000000000))*(cj1)); -IkReal x210=((IkReal(1.00000000000000))*(sj1)); -IkReal x211=((new_r10)*(sj2)); -IkReal x212=((new_r11)*(sj2)); -IkReal x213=((cj2)*(new_r00)); -IkReal x214=((IkReal(1.00000000000000))*(x207)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x207)*(x210)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x206)*(x210)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x206)+(((cj2)*(new_r10)))); -evalcond[3]=((((new_r01)*(sj2)))+(((cj2)*(new_r11)))+(((IkReal(-1.00000000000000))*(x214)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x207)*(x209)))+(x213)+(((IkReal(-1.00000000000000))*(x211)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x206)*(x209)))+(x208)+(((IkReal(-1.00000000000000))*(x212)))); -evalcond[6]=((((new_r20)*(sj1)))+(((cj1)*(x213)))+(((IkReal(-1.00000000000000))*(x214)))+(((IkReal(-1.00000000000000))*(x209)*(x211)))); -evalcond[7]=((((cj1)*(x208)))+(((new_r21)*(sj1)))+(((IkReal(-1.00000000000000))*(x206)))+(((IkReal(-1.00000000000000))*(x209)*(x212)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((gconst12)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j0array[0]=IKatan2(((gconst12)*(new_r21)), ((gconst12)*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[2]; -IkReal x215=((IkReal(1.00000000000000))*(sj1)); -evalcond[0]=((new_r20)+(((IkReal(-1.00000000000000))*(x215)*(IKcos(j0))))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x215)*(IKsin(j0))))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst17; -gconst17=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst16; -gconst16=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x216=((gconst16)*(sj0)); -if( IKabs(((new_r12)*(x216))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x216))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x216)), ((IkReal(-1.00000000000000))*(new_r02)*(x216))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[12]; -IkReal x217=IKsin(j2); -IkReal x218=IKcos(j2); -IkReal x219=((IkReal(1.00000000000000))*(cj0)); -IkReal x220=((IkReal(1.00000000000000))*(cj1)); -IkReal x221=((IkReal(1.00000000000000))*(x217)); -IkReal x222=((new_r01)*(x218)); -IkReal x223=((new_r00)*(x218)); -IkReal x224=((new_r02)*(x218)); -evalcond[0]=((((new_r02)*(x217)))+(((new_r12)*(x218)))); -evalcond[1]=((sj0)+(((new_r00)*(x217)))+(((new_r10)*(x218)))); -evalcond[2]=((sj1)+(x224)+(((IkReal(-1.00000000000000))*(new_r12)*(x221)))); -evalcond[3]=((((new_r01)*(x217)))+(((IkReal(-1.00000000000000))*(x219)))+(((new_r11)*(x218)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(cj1)*(x219)))+(((IkReal(-1.00000000000000))*(new_r10)*(x221)))+(x223)); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x221)))+(((IkReal(-1.00000000000000))*(sj0)*(x220)))+(x222)); -evalcond[6]=((((IkReal(-1.00000000000000))*(new_r12)*(x217)*(x220)))+(((cj1)*(x224)))+(((new_r22)*(sj1)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(new_r10)*(sj1)*(x221)))+(((sj1)*(x223)))+(((IkReal(-1.00000000000000))*(new_r20)*(x220)))); -evalcond[8]=((((sj1)*(x222)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj1)*(x221)))+(((IkReal(-1.00000000000000))*(new_r21)*(x220)))); -evalcond[9]=((IkReal(1.00000000000000))+(((sj1)*(x224)))+(((IkReal(-1.00000000000000))*(new_r22)*(x220)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj1)*(x221)))); -evalcond[10]=((((new_r20)*(sj1)))+(((cj1)*(x223)))+(((IkReal(-1.00000000000000))*(new_r10)*(x217)*(x220)))+(((IkReal(-1.00000000000000))*(x219)))); -evalcond[11]=((((IkReal(-1.00000000000000))*(sj0)))+(((cj1)*(x222)))+(((IkReal(-1.00000000000000))*(new_r11)*(x217)*(x220)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x225=((gconst17)*(sj1)); -if( IKabs(((new_r12)*(x225))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x225))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x225)), ((IkReal(-1.00000000000000))*(new_r02)*(x225))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[12]; -IkReal x226=IKsin(j2); -IkReal x227=IKcos(j2); -IkReal x228=((IkReal(1.00000000000000))*(cj0)); -IkReal x229=((IkReal(1.00000000000000))*(cj1)); -IkReal x230=((IkReal(1.00000000000000))*(x226)); -IkReal x231=((new_r01)*(x227)); -IkReal x232=((new_r00)*(x227)); -IkReal x233=((new_r02)*(x227)); -evalcond[0]=((((new_r02)*(x226)))+(((new_r12)*(x227)))); -evalcond[1]=((((new_r00)*(x226)))+(sj0)+(((new_r10)*(x227)))); -evalcond[2]=((sj1)+(((IkReal(-1.00000000000000))*(new_r12)*(x230)))+(x233)); -evalcond[3]=((((new_r01)*(x226)))+(((new_r11)*(x227)))+(((IkReal(-1.00000000000000))*(x228)))); -evalcond[4]=((x232)+(((IkReal(-1.00000000000000))*(cj1)*(x228)))+(((IkReal(-1.00000000000000))*(new_r10)*(x230)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(sj0)*(x229)))+(x231)+(((IkReal(-1.00000000000000))*(new_r11)*(x230)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(new_r12)*(x226)*(x229)))+(((cj1)*(x233)))+(((new_r22)*(sj1)))); -evalcond[7]=((((sj1)*(x232)))+(((IkReal(-1.00000000000000))*(new_r20)*(x229)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj1)*(x230)))); -evalcond[8]=((((sj1)*(x231)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj1)*(x230)))+(((IkReal(-1.00000000000000))*(new_r21)*(x229)))); -evalcond[9]=((IkReal(1.00000000000000))+(((sj1)*(x233)))+(((IkReal(-1.00000000000000))*(new_r22)*(x229)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj1)*(x230)))); -evalcond[10]=((((cj1)*(x232)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(new_r10)*(x226)*(x229)))+(((IkReal(-1.00000000000000))*(x228)))); -evalcond[11]=((((cj1)*(x231)))+(((IkReal(-1.00000000000000))*(sj0)))+(((IkReal(-1.00000000000000))*(new_r11)*(x226)*(x229)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} -} -} -} -}}; - - -/// 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 const char* GetKinematicsHash() { return ""; } - -IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } - -#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; + if (argc != 12 + GetNumFreeParameters() + 1) + { + printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" + "Returns the ik solutions given the transformation of the end effector specified by\n" + "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n" + "There are %d free parameters that have to be specified.\n\n", + GetNumFreeParameters()); + return 1; + } + + IkSolutionList solutions; + std::vector vfree(GetNumFreeParameters()); + IkReal eerot[9], eetrans[3]; + eerot[0] = atof(argv[1]); + eerot[1] = atof(argv[2]); + eerot[2] = atof(argv[3]); + eetrans[0] = atof(argv[4]); + eerot[3] = atof(argv[5]); + eerot[4] = atof(argv[6]); + eerot[5] = atof(argv[7]); + eetrans[1] = atof(argv[8]); + eerot[6] = atof(argv[9]); + eerot[7] = atof(argv[10]); + eerot[8] = atof(argv[11]); + eetrans[2] = atof(argv[12]); + for (std::size_t i = 0; i < vfree.size(); ++i) + vfree[i] = atof(argv[13 + i]); + bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); + + if (!bSuccess) + { + fprintf(stderr, "Failed to get ik solution\n"); + return -1; + } + + printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions()); + std::vector solvalues(GetNumJoints()); + for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) + { + const IkSolutionBase& sol = solutions.GetSolution(i); + printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size()); + std::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); + for (std::size_t j = 0; j < solvalues.size(); ++j) + printf("%.15f, ", solvalues[j]); + printf("\n"); + } + return 0; } #endif diff --git a/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/ikfast.h b/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/ikfast.h index 4f2b4371..803e6cf3 100644 --- a/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/ikfast.h +++ b/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/ikfast.h @@ -16,18 +16,23 @@ The ikfast inverse kinematics compiler is part of OpenRAVE. The file is divided into two sections: - - Common - the abstract classes section that all ikfast share regardless of their settings - - Library Specific - the library-specific definitions, which depends on the precision/settings that the library was compiled with + - Common - the abstract classes section that all ikfast share regardless of their + settings + - Library Specific - the library-specific definitions, which depends on the + precision/settings that the library was compiled with The defines are as follows, they are also used for the ikfast C++ class: - IKFAST_HEADER_COMMON - common classes - - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off + - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is + off - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility. - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY - - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected. + - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and + other invalid conditions are detected. - IKFAST_REAL - Use to force a custom real number type for IkReal. - - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded. + - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function + is excluded. */ #include @@ -39,227 +44,248 @@ /// should be the same as ikfast.__version__ #define IKFAST_VERSION 61 -namespace ikfast { +namespace ikfast +{ /// \brief holds the solution for a single dof -template -class IkSingleDOFSolutionBase +template class IkSingleDOFSolutionBase { public: - IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) { - indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; - } - T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset - signed char freeind; ///< if >= 0, mimics another joint - unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider - unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself - unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root + IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) + { + indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; + } + T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset + signed char freeind; ///< if >= 0, mimics another joint + unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider + unsigned char + maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself + unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it + ///came from. sometimes a solution can be repeated for different + ///indices. store at least another repeated root }; /// \brief The discrete solutions are returned in this structure. /// /// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. -/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is: -template -class IkSolutionBase +/// Stores all these solutions in the form of free variables that the user has to set when querying +/// the solution. Its prototype is: +template class IkSolutionBase { public: - virtual ~IkSolutionBase() { - } - /// \brief gets a concrete solution - /// - /// \param[out] solution the result - /// \param[in] freevalues values for the free parameters \se GetFree - virtual void GetSolution(T* solution, const T* freevalues) const = 0; - - /// \brief std::vector version of \ref GetSolution - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } - - /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned - /// - /// \return vector of indices indicating the free parameters - virtual const std::vector& GetFree() const = 0; - - /// \brief the dof of the solution - virtual const int GetDOF() const = 0; + virtual ~IkSolutionBase() {} + /// \brief gets a concrete solution + /// + /// \param[out] solution the result + /// \param[in] freevalues values for the free parameters \se GetFree + virtual void GetSolution(T* solution, const T* freevalues) const = 0; + + /// \brief std::vector version of \ref GetSolution + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + } + + /// \brief Gets the indices of the configuration space that have to be preset before a full + /// solution can be returned + /// + /// \return vector of indices indicating the free parameters + virtual const std::vector& GetFree() const = 0; + + /// \brief the dof of the solution + virtual const int GetDOF() const = 0; }; /// \brief manages all the solutions -template -class IkSolutionListBase +template class IkSolutionListBase { public: - virtual ~IkSolutionListBase() { - } + virtual ~IkSolutionListBase() {} - /// \brief add one solution and return its index for later retrieval - /// - /// \param vinfos Solution data for each degree of freedom of the manipulator - /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; + /// \brief add one solution and return its index for later retrieval + /// + /// \param vinfos Solution data for each degree of freedom of the manipulator + /// \param vfree If the solution represents an infinite space, holds free parameters of the + /// solution that users can freely set. + virtual size_t AddSolution(const std::vector >& vinfos, + const std::vector& vfree) = 0; - /// \brief returns the solution pointer - virtual const IkSolutionBase& GetSolution(size_t index) const = 0; + /// \brief returns the solution pointer + virtual const IkSolutionBase& GetSolution(size_t index) const = 0; - /// \brief returns the number of solutions stored - virtual size_t GetNumSolutions() const = 0; + /// \brief returns the number of solutions stored + virtual size_t GetNumSolutions() const = 0; - /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated. - virtual void Clear() = 0; + /// \brief clears all current solutions, note that any memory addresses returned from \ref + /// GetSolution will be invalidated. + virtual void Clear() = 0; }; /// \brief holds function pointers for all the exported functions of ikfast -template -class IkFastFunctions +template class IkFastFunctions { public: - IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) { - } - virtual ~IkFastFunctions() { - } - typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); - ComputeIkFn _ComputeIk; - typedef void (*ComputeFkFn)(const T*, T*, T*); - ComputeFkFn _ComputeFk; - typedef int (*GetNumFreeParametersFn)(); - GetNumFreeParametersFn _GetNumFreeParameters; - typedef int* (*GetFreeParametersFn)(); - GetFreeParametersFn _GetFreeParameters; - typedef int (*GetNumJointsFn)(); - GetNumJointsFn _GetNumJoints; - typedef int (*GetIkRealSizeFn)(); - GetIkRealSizeFn _GetIkRealSize; - typedef const char* (*GetIkFastVersionFn)(); - GetIkFastVersionFn _GetIkFastVersion; - typedef int (*GetIkTypeFn)(); - GetIkTypeFn _GetIkType; - typedef const char* (*GetKinematicsHashFn)(); - GetKinematicsHashFn _GetKinematicsHash; + IkFastFunctions() + : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), + _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), + _GetKinematicsHash(NULL) + { + } + virtual ~IkFastFunctions() {} + typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); + ComputeIkFn _ComputeIk; + typedef void (*ComputeFkFn)(const T*, T*, T*); + ComputeFkFn _ComputeFk; + typedef int (*GetNumFreeParametersFn)(); + GetNumFreeParametersFn _GetNumFreeParameters; + typedef int* (*GetFreeParametersFn)(); + GetFreeParametersFn _GetFreeParameters; + typedef int (*GetNumJointsFn)(); + GetNumJointsFn _GetNumJoints; + typedef int (*GetIkRealSizeFn)(); + GetIkRealSizeFn _GetIkRealSize; + typedef const char* (*GetIkFastVersionFn)(); + GetIkFastVersionFn _GetIkFastVersion; + typedef int (*GetIkTypeFn)(); + GetIkTypeFn _GetIkType; + typedef const char* (*GetKinematicsHashFn)(); + GetKinematicsHashFn _GetKinematicsHash; }; // Implementations of the abstract classes, user doesn't need to use them /// \brief Default implementation of \ref IkSolutionBase -template -class IkSolution : public IkSolutionBase +template class IkSolution : public IkSolutionBase { public: - IkSolution(const std::vector >& vinfos, const std::vector& vfree) { - _vbasesol = vinfos; - _vfree = vfree; - } - - virtual void GetSolution(T* solution, const T* freevalues) const { - for(std::size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].freeind < 0 ) - solution[i] = _vbasesol[i].foffset; - else { - solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset; - if( solution[i] > T(3.14159265358979) ) { - solution[i] -= T(6.28318530717959); - } - else if( solution[i] < T(-3.14159265358979) ) { - solution[i] += T(6.28318530717959); - } - } + IkSolution(const std::vector >& vinfos, const std::vector& vfree) + { + _vbasesol = vinfos; + _vfree = vfree; + } + + virtual void GetSolution(T* solution, const T* freevalues) const + { + for (std::size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].freeind < 0) + solution[i] = _vbasesol[i].foffset; + else + { + solution[i] = freevalues[_vbasesol[i].freeind] * _vbasesol[i].fmul + _vbasesol[i].foffset; + if (solution[i] > T(3.14159265358979)) + { + solution[i] -= T(6.28318530717959); } + else if (solution[i] < T(-3.14159265358979)) + { + solution[i] += T(6.28318530717959); + } + } } + } - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } + virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const + { + solution.resize(GetDOF()); + GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); + } - virtual const std::vector& GetFree() const { - return _vfree; - } - virtual const int GetDOF() const { - return static_cast(_vbasesol.size()); - } + virtual const std::vector& GetFree() const { return _vfree; } + virtual const int GetDOF() const { return static_cast(_vbasesol.size()); } - virtual void Validate() const { - for(size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].maxsolutions == (unsigned char)-1) { - throw std::runtime_error("max solutions for joint not initialized"); - } - if( _vbasesol[i].maxsolutions > 0 ) { - if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("index >= max solutions for joint"); - } - if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("2nd index >= max solutions for joint"); - } - } + virtual void Validate() const + { + for (size_t i = 0; i < _vbasesol.size(); ++i) + { + if (_vbasesol[i].maxsolutions == (unsigned char)-1) + { + throw std::runtime_error("max solutions for joint not initialized"); + } + if (_vbasesol[i].maxsolutions > 0) + { + if (_vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("index >= max solutions for joint"); } + if (_vbasesol[i].indices[1] != (unsigned char)-1 && + _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions) + { + throw std::runtime_error("2nd index >= max solutions for joint"); + } + } } + } - virtual void GetSolutionIndices(std::vector& v) const { - v.resize(0); - v.push_back(0); - for(int i = (int)_vbasesol.size()-1; i >= 0; --i) { - if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) { - for(size_t j = 0; j < v.size(); ++j) { - v[j] *= _vbasesol[i].maxsolutions; - } - size_t orgsize=v.size(); - if( _vbasesol[i].indices[1] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v.push_back(v[j]+_vbasesol[i].indices[1]); - } - } - if( _vbasesol[i].indices[0] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v[j] += _vbasesol[i].indices[0]; - } - } - } + virtual void GetSolutionIndices(std::vector& v) const + { + v.resize(0); + v.push_back(0); + for (int i = (int)_vbasesol.size() - 1; i >= 0; --i) + { + if (_vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1) + { + for (size_t j = 0; j < v.size(); ++j) + { + v[j] *= _vbasesol[i].maxsolutions; } + size_t orgsize = v.size(); + if (_vbasesol[i].indices[1] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v.push_back(v[j] + _vbasesol[i].indices[1]); + } + } + if (_vbasesol[i].indices[0] != (unsigned char)-1) + { + for (size_t j = 0; j < orgsize; ++j) + { + v[j] += _vbasesol[i].indices[0]; + } + } + } } + } - std::vector< IkSingleDOFSolutionBase > _vbasesol; ///< solution and their offsets if joints are mimiced - std::vector _vfree; + std::vector > + _vbasesol; ///< solution and their offsets if joints are mimiced + std::vector _vfree; }; /// \brief Default implementation of \ref IkSolutionListBase -template -class IkSolutionList : public IkSolutionListBase +template class IkSolutionList : public IkSolutionListBase { public: - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) + virtual size_t AddSolution(const std::vector >& vinfos, + const std::vector& vfree) + { + size_t index = _listsolutions.size(); + _listsolutions.push_back(IkSolution(vinfos, vfree)); + return index; + } + + virtual const IkSolutionBase& GetSolution(size_t index) const + { + if (index >= _listsolutions.size()) { - size_t index = _listsolutions.size(); - _listsolutions.push_back(IkSolution(vinfos,vfree)); - return index; + throw std::runtime_error("GetSolution index is invalid"); } + typename std::list >::const_iterator it = _listsolutions.begin(); + std::advance(it, index); + return *it; + } - virtual const IkSolutionBase& GetSolution(size_t index) const - { - if( index >= _listsolutions.size() ) { - throw std::runtime_error("GetSolution index is invalid"); - } - typename std::list< IkSolution >::const_iterator it = _listsolutions.begin(); - std::advance(it,index); - return *it; - } - - virtual size_t GetNumSolutions() const { - return _listsolutions.size(); - } + virtual size_t GetNumSolutions() const { return _listsolutions.size(); } - virtual void Clear() { - _listsolutions.clear(); - } + virtual void Clear() { _listsolutions.clear(); } protected: - std::list< IkSolution > _listsolutions; + std::list > _listsolutions; }; - } - // The following code is dependent on the C++ library linking with. #ifdef IKFAST_HAS_LIBRARY @@ -275,7 +301,8 @@ class IkSolutionList : public IkSolutionListBase #endif #ifdef IKFAST_NAMESPACE -namespace IKFAST_NAMESPACE { +namespace IKFAST_NAMESPACE +{ #endif #ifdef IKFAST_REAL @@ -286,16 +313,22 @@ typedef double IkReal; /** \brief Computes all IK solutions given a end effector coordinates and the free joints. - - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. + - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is + the orientation. - ``eerot`` - For **Transform6D** it is 9 values for the 3x3 rotation matrix. - - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. - - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle. - - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. + - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent + the target direction. + - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** + the first value represents the angle. + - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation + inside the end effector coordinate system. */ -IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase& solutions); +IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, + ikfast::IkSolutionListBase& solutions); -/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. +/// \brief Computes the end effector coordinates given the joint values. This function is used to +/// double check ik. IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); /// \brief returns the number of free parameters users has to set apriori diff --git a/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_moveit_plugin.hpp b/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_moveit_plugin.hpp index d33208cf..864df929 100644 --- a/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_moveit_plugin.hpp +++ b/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_moveit_plugin.hpp @@ -2,7 +2,8 @@ * * Software License Agreement (BSD License) * - * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias Lüdtke, Fraunhofer IPA + * 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 @@ -56,15 +57,20 @@ // Code generated by IKFast56/61 #include -namespace motoman_sia20d_ikfast_manipulator { +namespace motoman_sia20d_ikfast_manipulator +{ const static std::string MOTOMAN_BASE_LINK = "base_link"; -const static std::string MOTOMAN_TIP_LINK ="tool0"; +const static std::string MOTOMAN_TIP_LINK = "tool0"; // 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 }; +enum SEARCH_MODE +{ + OPTIMIZE_FREE_JOINT = 1, + OPTIMIZE_MAX_JOINT = 2 +}; namespace ikfast_kinematics_plugin { @@ -76,52 +82,79 @@ namespace ikfast_kinematics_plugin /// 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 +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 }; - class IKFastKinematicsPlugin : public kinematics::KinematicsBase { @@ -139,11 +172,10 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase const std::vector& getLinkNames() const { return link_names_; } public: - /** @class * @brief Interface for an IKFast kinematics plugin */ - IKFastKinematicsPlugin():active_(false){} + IKFastKinematicsPlugin() : active_(false) {} /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it @@ -155,78 +187,82 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase */ // Returns the first IK solution that is within joint limits, this is called by get_ik() service - 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; + 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, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * @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; + 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 + * @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; + 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 + * @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; + 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 + * @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; + 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 @@ -236,61 +272,62 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase * @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; + bool getPositionFK(const std::vector& link_names, + const std::vector& joint_angles, + std::vector& poses) const; protected: - - bool initialize(const std::string &robot_description, - const std::string& group_name, - const std::string& base_name, - const std::string& tip_name, + 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; + 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; - - 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; + void getSolution(const IkSolutionList& solutions, 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; }; // 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) +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) { - ROS_INFO_STREAM("FastIK Plugin for motoman_sia20d received base_link: '"<< - base_name<< "' and tip_link: '"< 1) + if (free_params_.size() > 1) { ROS_FATAL("Only one free joint paramter supported!"); return false; @@ -299,44 +336,47 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, urdf::Model robot_model; std::string xml_string; - std::string urdf_xml,full_urdf_xml; - node_handle.param("urdf_xml",urdf_xml,robot_description); - node_handle.searchParam(urdf_xml,full_urdf_xml); + std::string urdf_xml, full_urdf_xml; + node_handle.param("urdf_xml", urdf_xml, robot_description); + node_handle.searchParam(urdf_xml, full_urdf_xml); - ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server"); + ROS_DEBUG_NAMED("ikfast", "Reading xml file from parameter server"); if (!node_handle.getParam(full_urdf_xml, xml_string)) { - ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str()); + ROS_FATAL_NAMED("ikfast", "Could not load the xml from parameter server: %s", urdf_xml.c_str()); return false; } - node_handle.param(full_urdf_xml,xml_string,std::string()); + node_handle.param(full_urdf_xml, xml_string, std::string()); robot_model.initString(xml_string); - ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Reading joints and links from URDF"); - boost::shared_ptr link = boost::const_pointer_cast(robot_model.getLink(getTipFrame())); - while(link->name != base_frame_ && joint_names_.size() <= num_joints_) + boost::shared_ptr link = + boost::const_pointer_cast(robot_model.getLink(getTipFrame())); + while (link->name != base_frame_ && joint_names_.size() <= num_joints_) { - ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str()); + ROS_DEBUG_NAMED("ikfast", "Link %s", link->name.c_str()); link_names_.push_back(link->name); boost::shared_ptr joint = link->parent_joint; - if(joint) + if (joint) { if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { - ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name ); + ROS_DEBUG_STREAM_NAMED("ikfast", "Adding joint " << joint->name); joint_names_.push_back(joint->name); float lower, upper; int hasLimits; - if ( joint->type != urdf::Joint::CONTINUOUS ) + if (joint->type != urdf::Joint::CONTINUOUS) { - if(joint->safety) + if (joint->safety) { lower = joint->safety->soft_lower_limit; upper = joint->safety->soft_upper_limit; - } else { + } + else + { lower = joint->limits->lower; upper = joint->limits->upper; } @@ -348,7 +388,7 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, upper = M_PI; hasLimits = 0; } - if(hasLimits) + if (hasLimits) { joint_has_limits_vector_.push_back(true); joint_min_vector_.push_back(lower); @@ -361,39 +401,45 @@ bool IKFastKinematicsPlugin::initialize(const std::string &robot_description, joint_max_vector_.push_back(M_PI); } } - } else + } + else { - ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str()); + ROS_WARN_NAMED("ikfast", "no joint corresponding to %s", link->name.c_str()); } link = link->getParent(); } - if(joint_names_.size() != num_joints_) + if (joint_names_.size() != num_joints_) { - ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_); + ROS_FATAL_STREAM_NAMED("ikfast", "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(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 &vfree, IkSolutionList &solutions) const +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[0] = pose_frame.p[0]; //-.18; trans[1] = pose_frame.p[1]; trans[2] = pose_frame.p[2]; @@ -402,99 +448,110 @@ int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector 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**, and **TranslationZAxisAngle4D**, the first value represents the angle. - ROS_ERROR_NAMED("ikfast", "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("ikfast", "IK for this IkParameterizationType not implemented yet."); - return 0; - - case IKP_Rotation3D: - case IKP_Lookat3D: - case IKP_TranslationXY2D: - case IKP_TranslationXYOrientation3D: - case IKP_TranslationXAxisAngleZNorm4D: - case IKP_TranslationYAxisAngleXNorm4D: - case IKP_TranslationZAxisAngleYNorm4D: - ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); - return 0; - - default: - ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?"); - return 0; + 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**, and + // **TranslationZAxisAngle4D**, the first value represents the angle. + ROS_ERROR_NAMED("ikfast", "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("ikfast", "IK for this IkParameterizationType not implemented yet."); + return 0; + + case IKP_Rotation3D: + case IKP_Lookat3D: + case IKP_TranslationXY2D: + case IKP_TranslationXYOrientation3D: + case IKP_TranslationXAxisAngleZNorm4D: + case IKP_TranslationYAxisAngleXNorm4D: + case IKP_TranslationZAxisAngleYNorm4D: + ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet."); + return 0; + + default: + ROS_ERROR_NAMED("ikfast", "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 +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::vector vsolfree(sol.GetFree().size()); + sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); // std::cout << "solution " << i << ":" ; // for(int j=0;j &ik_seed_state, std::vector &solution) const +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) + 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(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; } - 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]); } @@ -524,48 +581,53 @@ double IKFastKinematicsPlugin::harmonize(const std::vector &ik_seed_stat // } // } -void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList &solutions, const std::vector &ik_seed_state, std::vector &solution) const +void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList& solutions, + const std::vector& ik_seed_state, + std::vector& solution) const { double mindist = DBL_MAX; int minindex = -1; std::vector sol; // IKFast56/61 - for(size_t i=0; i < solutions.GetNumSolutions(); ++i) + for (size_t i = 0; i < solutions.GetNumSolutions(); ++i) { - getSolution(solutions, i,sol); + getSolution(solutions, i, sol); double dist = harmonize(ik_seed_state, sol); - ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist); - //std::cout << "dist[" << i << "]= " << dist << std::endl; - if(minindex == -1 || dist &ik_seed_state, - double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const +bool IKFastKinematicsPlugin::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) const { - const IKCallbackFn solution_callback = 0; + 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); + 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 + +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); + 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 +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); + 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 +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("ikfast","searchPositionIK"); + ROS_DEBUG_STREAM_NAMED("ikfast", "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) + if (free_params_.size() == 0) { - ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints"); + ROS_DEBUG_STREAM_NAMED("ikfast", "No need to search since no free params/redundant joints"); // Find first IK solution, within joint limits - if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code)) + if (!getPositionIK(ik_pose, ik_seed_state, solution, error_code)) { - ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever"); + ROS_DEBUG_STREAM_NAMED("ikfast", "No solution whatsoever"); error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } // check for collisions if a callback is provided - if( !solution_callback.empty() ) + if (!solution_callback.empty()) { solution_callback(ik_pose, solution, error_code); - if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) + if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback"); return true; } else { - ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code); + ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code); return false; } } @@ -744,33 +782,36 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose // ------------------------------------------------------------------------------------------------- // Error Checking - if(!active_) + if (!active_) { - ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active"); + ROS_ERROR_STREAM_NAMED("ikfast", "Kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } - if(ik_seed_state.size() != num_joints_) + if (ik_seed_state.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); + ROS_ERROR_STREAM_NAMED("ikfast", "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_) + if (!consistency_limits.empty() && consistency_limits.size() != num_joints_) { - ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size()); + ROS_ERROR_STREAM_NAMED("ikfast", "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); + tf::poseMsgToKDL(ik_pose, frame); std::vector vfree(free_params_.size()); @@ -785,66 +826,78 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose int num_positive_increments; int num_negative_increments; - if(!consistency_limits.empty()) + 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]]); + 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_); + 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_; + 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("ikfast","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("ikfast", "Large search space, consider increasing the search discretization"); - + ROS_DEBUG_STREAM_NAMED("ikfast", "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("ikfast", + "Large search space, consider increasing the search discretization"); + double best_costs = -1.0; std::vector best_solution; int nattempts = 0, nvalid = 0; - while(true) + while (true) { IkSolutionList solutions; - int numsol = solve(frame,vfree, solutions); + int numsol = solve(frame, vfree, solutions); - ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast"); - //ROS_INFO("%f",vfree[0]); + // ROS_INFO("%f",vfree[0]); - if( numsol > 0 ) + if (numsol > 0) { - for(int s = 0; s < numsol; ++s) + for (int s = 0; s < numsol; ++s) { nattempts++; std::vector sol; - getSolution(solutions,s,sol); + getSolution(solutions, s, sol); bool obeys_limits = true; - for(unsigned int i = 0; i < sol.size(); i++) + 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])) + 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("ikfast","Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); + // ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " << + // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << + // joint_max_vector_[i]); } - if(obeys_limits) + if (obeys_limits) { - getSolution(solutions,s,solution); + getSolution(solutions, s, solution); // This solution is within joint limits, now check if in collision (if callback provided) - if(!solution_callback.empty()) + if (!solution_callback.empty()) { solution_callback(ik_pose, solution, error_code); } @@ -853,14 +906,14 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose error_code.val = error_code.SUCCESS; } - if(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++) + for (unsigned int i = 0; i < solution.size(); i++) { double d = fabs(ik_seed_state[i] - solution[i]); if (d > costs) @@ -880,15 +933,16 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose } } - if(!getCount(counter, num_positive_increments, -num_negative_increments)) + 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("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]); + vfree[0] = initial_guess + search_discretization_ * counter; + // ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " + // << vfree[0]); } ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts); @@ -906,61 +960,66 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose } // 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 +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("ikfast","getPositionIK"); + ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK"); - if(!active_) + if (!active_) { - ROS_ERROR("kinematics not active"); + ROS_ERROR("kinematics not active"); return false; } std::vector vfree(free_params_.size()); - for(std::size_t i = 0; i < free_params_.size(); ++i) + 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 + 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); + tf::poseMsgToKDL(ik_pose, frame); IkSolutionList solutions; - int numsol = solve(frame,vfree,solutions); + int numsol = solve(frame, vfree, solutions); - ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast"); + ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast"); - if(numsol) + if (numsol) { - for(int s = 0; s < numsol; ++s) + for (int s = 0; s < numsol; ++s) { std::vector sol; - getSolution(solutions,s,sol); - ROS_DEBUG_NAMED("ikfast","Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]); + getSolution(solutions, s, sol); + ROS_DEBUG_NAMED("ikfast", "Sol %d: %e %e %e %e %e %e", s, sol[0], sol[1], sol[2], + sol[3], sol[4], sol[5]); bool obeys_limits = true; - for(unsigned int i = 0; i < sol.size(); i++) + 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)) ) ) + 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("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << " being " << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + ROS_DEBUG_STREAM_NAMED("ikfast", "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) + if (obeys_limits) { // All elements of solution obey limits - getSolution(solutions,s,solution); + getSolution(solutions, s, solution); error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; return true; } @@ -968,16 +1027,14 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, } else { - ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution"); + ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution"); } error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; return false; } - - } // end namespace ikfast_kinematics_plugin -} // end namespace motoman_sia20d_ikfast_manipulator +} // end namespace motoman_sia20d_ikfast_manipulator -#endif //MOTOMAN_SIA20D_MANIPULATOR_IKFAST_MOVEIT_PLUGIN +#endif // MOTOMAN_SIA20D_MANIPULATOR_IKFAST_MOVEIT_PLUGIN diff --git a/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_solver.hpp b/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_solver.hpp index 4cccb702..fb881d58 100644 --- a/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_solver.hpp +++ b/motoman_sia20d_ikfast_manipulator/include/motoman_sia20d_ikfast_manipulator/motoman_sia20d_manipulator_ikfast_solver.hpp @@ -5,7 +5,7 @@ /// 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. @@ -16,7 +16,8 @@ /// 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 +/// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o +/// libik.so ik.cpp #ifndef MOTOMAN_SIA20D_MANIPULATOR_IKFAST_SOLVER #define MOTOMAN_SIA20D_MANIPULATOR_IKFAST_SOLVER @@ -27,7 +28,7 @@ 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==61); +IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61); #include #include @@ -53,7 +54,16 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #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()); } } +#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 @@ -63,9 +73,9 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); #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) +#define IK2PI ((IkReal)6.28318530717959) +#define IKPI ((IkReal)3.14159265358979) +#define IKPI_2 ((IkReal)1.57079632679490) #ifdef _MSC_VER #ifndef isnan @@ -75,25 +85,32 @@ IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); // 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); +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 { +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 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); } @@ -115,50 +132,68 @@ inline double IKlog(double f) { return log(f); } 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); + 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); + 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); + 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); + 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); + 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); + 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); } @@ -166,4088 +201,6024 @@ 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 IKatan2(float fy, float fx) { - if( std::isnan(fy) ) { - IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned - return float(IKPI_2); - } - else if( std::isnan(fx) ) { - return 0; - } - return atan2f(fy,fx); -} -inline double IKatan2(double fy, double fx) { - if( std::isnan(fy) ) { - IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned - return IKPI_2; - } - else if( std::isnan(fx) ) { - return 0; - } - return atan2(fy,fx); -} - -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; -} - -/// 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,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63; -x0=IKcos(j[0]); -x1=IKcos(j[3]); -x2=IKcos(j[1]); -x3=IKcos(j[2]); -x4=IKsin(j[1]); -x5=IKsin(j[3]); -x6=IKsin(j[0]); -x7=IKsin(j[2]); -x8=IKsin(j[4]); -x9=IKcos(j[4]); -x10=IKcos(j[6]); -x11=IKsin(j[6]); -x12=IKsin(j[5]); -x13=IKcos(j[5]); -x14=((IkReal(1.00000000000000))*(x7)); -x15=((IkReal(1.00000000000000))*(x1)); -x16=((IkReal(0.180000000000000))*(x9)); -x17=((IkReal(0.420000000000000))*(x2)); -x18=((IkReal(1.00000000000000))*(x9)); -x19=((IkReal(1.00000000000000))*(x2)); -x20=((IkReal(0.180000000000000))*(x1)); -x21=((IkReal(1.00000000000000))*(x13)); -x22=((IkReal(1.00000000000000))*(x8)); -x23=((IkReal(0.180000000000000))*(x5)); -x24=((IkReal(1.00000000000000))*(x5)); -x25=((IkReal(0.420000000000000))*(x1)); -x26=((IkReal(1.00000000000000))*(x12)); -x27=((x0)*(x7)); -x28=((IkReal(-1.00000000000000))*(x13)); -x29=((x3)*(x6)); -x30=((x0)*(x4)); -x31=((x4)*(x8)); -x32=((x4)*(x6)); -x33=((x6)*(x7)); -x34=((x0)*(x3)); -x35=((x3)*(x4)); -x36=((IkReal(-1.00000000000000))*(x12)); -x37=((x14)*(x6)); -x38=((x15)*(x35)); -x39=((x14)*(x4)*(x9)); -x40=((x14)*(x31)); -x41=((((IkReal(-1.00000000000000))*(x37)))+(((x2)*(x34)))); -x42=((((x2)*(x27)))+(x29)); -x43=((((x2)*(x29)))+(x27)); -x44=((((IkReal(-1.00000000000000))*(x34)))+(((x2)*(x33)))); -x45=((((IkReal(-1.00000000000000))*(x38)))+(((x2)*(x5)))); -x46=((((x1)*(x2)))+(((x35)*(x5)))); -x47=((((IkReal(-1.00000000000000))*(x19)*(x34)))+(x37)); -x48=((((IkReal(-1.00000000000000))*(x19)*(x5)))+(x38)); -x49=((((IkReal(-1.00000000000000))*(x0)*(x14)))+(((IkReal(-1.00000000000000))*(x19)*(x29)))); -x50=((x42)*(x8)); -x51=((x1)*(x41)); -x52=((x44)*(x8)); -x53=((x49)*(x5)); -x54=((((x30)*(x5)))+(x51)); -x55=((((x32)*(x5)))+(((x1)*(x43)))); -x56=((((x47)*(x5)))+(((x1)*(x30)))); -x57=((((IkReal(-1.00000000000000))*(x15)*(x43)))+(((IkReal(-1.00000000000000))*(x24)*(x32)))); -x58=((x53)+(((x1)*(x32)))); -x59=((x54)*(x9)); -x60=((x59)+(x50)); -x61=((x52)+(((x55)*(x9)))); -x62=((((x21)*(((((IkReal(-1.00000000000000))*(x45)*(x9)))+(x40)))))+(((IkReal(-1.00000000000000))*(x26)*(x46)))); -x63=((x13)*(x60)); -eerot[0]=((((x10)*(((((x36)*(x56)))+(((x28)*(x60)))))))+(((IkReal(-1.00000000000000))*(x11)*(((((x18)*(x42)))+(((x22)*(((((IkReal(-1.00000000000000))*(x15)*(x41)))+(((IkReal(-1.00000000000000))*(x24)*(x30)))))))))))); -eerot[1]=((((x10)*(((((x42)*(x9)))+(((x8)*(((((IkReal(-1.00000000000000))*(x51)))+(((IkReal(-1.00000000000000))*(x30)*(x5)))))))))))+(((x11)*(((((IkReal(-1.00000000000000))*(x26)*(x56)))+(((IkReal(-1.00000000000000))*(x21)*(x60)))))))); -eerot[2]=((((x12)*(((((IkReal(-1.00000000000000))*(x18)*(x54)))+(((IkReal(-1.00000000000000))*(x22)*(x42)))))))+(((x13)*(x56)))); -eetrans[0]=((((x12)*(((((IkReal(-0.180000000000000))*(x50)))+(((IkReal(-1.00000000000000))*(x16)*(x54)))))))+(((IkReal(0.490000000000000))*(x30)))+(((x25)*(x30)))+(((x5)*(((((IkReal(0.420000000000000))*(x33)))+(((IkReal(-1.00000000000000))*(x17)*(x34)))))))+(((x13)*(((((x20)*(x30)))+(((x23)*(x47)))))))); -eerot[3]=((((x11)*(((((IkReal(-1.00000000000000))*(x18)*(x44)))+(((IkReal(-1.00000000000000))*(x22)*(x57)))))))+(((x10)*(((((x36)*(x58)))+(((x28)*(x61)))))))); -eerot[4]=((((x10)*(((((x57)*(x8)))+(((x44)*(x9)))))))+(((x11)*(((((IkReal(-1.00000000000000))*(x26)*(x58)))+(((IkReal(-1.00000000000000))*(x21)*(x61)))))))); -eerot[5]=((((x13)*(x58)))+(((x12)*(((((IkReal(-1.00000000000000))*(x18)*(x55)))+(((IkReal(-1.00000000000000))*(x22)*(x44)))))))); -eetrans[1]=((((x5)*(((((IkReal(-1.00000000000000))*(x17)*(x29)))+(((IkReal(-0.420000000000000))*(x27)))))))+(((x12)*(((((IkReal(-0.180000000000000))*(x52)))+(((IkReal(-1.00000000000000))*(x16)*(x55)))))))+(((IkReal(0.490000000000000))*(x32)))+(((x25)*(x32)))+(((x13)*(((((x20)*(x32)))+(((x23)*(x49)))))))); -eerot[6]=((((x11)*(((((IkReal(-1.00000000000000))*(x22)*(x48)))+(x39)))))+(((x10)*(x62)))); -eerot[7]=((((x11)*(x62)))+(((x10)*(((((IkReal(-1.00000000000000))*(x39)))+(((x48)*(x8)))))))); -eerot[8]=((((x12)*(((((IkReal(-1.00000000000000))*(x18)*(x45)))+(x40)))))+(((x13)*(x46)))); -eetrans[2]=((IkReal(0.410000000000000))+(((x1)*(x17)))+(((x13)*(((((x23)*(x35)))+(((x2)*(x20)))))))+(((x12)*(((((IkReal(0.180000000000000))*(x31)*(x7)))+(((IkReal(-1.00000000000000))*(x16)*(x45)))))))+(((IkReal(0.420000000000000))*(x35)*(x5)))+(((IkReal(0.490000000000000))*(x2)))); -} - -IKFAST_API int GetNumFreeParameters() { return 1; } -IKFAST_API int* GetFreeParameters() { static int freeparams[] = {4}; return freeparams; } -IKFAST_API int GetNumJoints() { return 7; } - -IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } - -IKFAST_API int GetIkType() { return 0x67000001; } - -class IKSolver { -public: -IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j5,cj5,sj5,htj5,j6,cj6,sj6,htj6,j4,cj4,sj4,htj4,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,_ij5[2], _nj5,_ij6[2], _nj6,_ij4[2], _nj4; - -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; j5=numeric_limits::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1; j6=numeric_limits::quiet_NaN(); _ij6[0] = -1; _ij6[1] = -1; _nj6 = -1; _ij4[0] = -1; _ij4[1] = -1; _nj4 = 0; -for(int dummyiter = 0; dummyiter < 1; ++dummyiter) { - solutions.Clear(); -j4=pfree[0]; cj4=cos(pfree[0]); sj4=sin(pfree[0]); -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=((IkReal(-1.00000000000000))*(r00)); -new_r01=r01; -new_r02=((IkReal(-1.00000000000000))*(r02)); -new_px=((((IkReal(-0.180000000000000))*(r02)))+(px)); -new_r10=((IkReal(-1.00000000000000))*(r10)); -new_r11=r11; -new_r12=((IkReal(-1.00000000000000))*(r12)); -new_py=((((IkReal(-0.180000000000000))*(r12)))+(py)); -new_r20=((IkReal(-1.00000000000000))*(r20)); -new_r21=r21; -new_r22=((IkReal(-1.00000000000000))*(r22)); -new_pz=((IkReal(-0.410000000000000))+(pz)+(((IkReal(-0.180000000000000))*(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; -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=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10)))); -rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00)))); -rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00)))); -rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11)))); -rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01)))); -rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01)))); -rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12)))); -rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02)))); -rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02)))); -{ -IkReal j3array[2], cj3array[2], sj3array[2]; -bool j3valid[2]={false}; -_nj3 = 2; -cj3array[0]=((IkReal(-1.01190476190476))+(((IkReal(2.42954324586978))*(pp)))); -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( std::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 dummyeval[1]; -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=((IkReal(1.00000000000000))+(((IkReal(2.33333333333333))*(cj3)))+(((IkReal(1.36111111111111))*((cj4)*(cj4))*((sj3)*(sj3))))+(((IkReal(1.36111111111111))*((cj3)*(cj3))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j5array[2], cj5array[2], sj5array[2]; -bool j5valid[2]={false}; -_nj5 = 2; -IkReal x64=((IkReal(-0.420000000000000))+(((IkReal(-0.490000000000000))*(cj3)))); -if( IKabs(x64) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(0.490000000000000))*(cj4)*(sj3))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x65=((IkReal(1.00000000000000))*(IKatan2(x64, ((IkReal(0.490000000000000))*(cj4)*(sj3))))); -if( ((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))) < (IkReal)-0.00001 ) - continue; -if( (((npz)*(((IKabs(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((npz)*(((IKabs(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x66=IKasin(((npz)*(((IKabs(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3)))))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((x64)*(x64))+(((IkReal(0.240100000000000))*((cj4)*(cj4))*((sj3)*(sj3))))))))):(IkReal)1.0e30)))); -j5array[0]=((x66)+(((IkReal(-1.00000000000000))*(x65)))); -sj5array[0]=IKsin(j5array[0]); -cj5array[0]=IKcos(j5array[0]); -j5array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x65)))+(((IkReal(-1.00000000000000))*(x66)))); -sj5array[1]=IKsin(j5array[1]); -cj5array[1]=IKcos(j5array[1]); -if( j5array[0] > IKPI ) -{ - j5array[0]-=IK2PI; -} -else if( j5array[0] < -IKPI ) -{ j5array[0]+=IK2PI; -} -j5valid[0] = true; -if( j5array[1] > IKPI ) -{ - j5array[1]-=IK2PI; -} -else if( j5array[1] < -IKPI ) -{ j5array[1]+=IK2PI; -} -j5valid[1] = true; -for(int ij5 = 0; ij5 < 2; ++ij5) +inline float IKsqrt(float f) { -if( !j5valid[ij5] ) -{ - continue; -} -_ij5[0] = ij5; _ij5[1] = -1; -for(int iij5 = ij5+1; iij5 < 2; ++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 dummyeval[1]; -IkReal gconst0; -IkReal x67=((IkReal(100.000000000000))*(sj5)); -gconst0=IKsign(((((x67)*((npx)*(npx))))+(((x67)*((npy)*(npy)))))); -dummyeval[0]=((((sj5)*((npy)*(npy))))+(((sj5)*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst1; -IkReal x68=((IkReal(2100.00000000000))*(sj5)); -gconst1=IKsign(((((x68)*((npx)*(npx))))+(((x68)*((npy)*(npy)))))); -dummyeval[0]=((((sj5)*((npy)*(npy))))+(((sj5)*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[5]; -IkReal x69=((IkReal(1.00000000000000))*(pp)); -IkReal x70=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(((IkReal(-0.490000000000000))*(cj3)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x69)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=x70; -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(-0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x69)))); -evalcond[4]=x70; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst2; -gconst2=IKsign(((((IkReal(-100.000000000000))*((npy)*(npy))))+(((IkReal(-100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((npy)*(npy))))+(((IkReal(-1.00000000000000))*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst3; -IkReal x71=((IkReal(100.000000000000))*(sj4)); -gconst3=IKsign(((((IkReal(-1.00000000000000))*(x71)*((npx)*(npx))))+(((IkReal(-1.00000000000000))*(x71)*((npy)*(npy)))))); -IkReal x72=((IkReal(1.00000000000000))*(sj4)); -dummyeval[0]=((((IkReal(-1.00000000000000))*(x72)*((npx)*(npx))))+(((IkReal(-1.00000000000000))*(x72)*((npy)*(npy))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[5]; -IkReal x73=((IkReal(1.00000000000000))*(pp)); -IkReal x74=x70; -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x73)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=x74; -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(-1.00000000000000))*(x73)))+(((IkReal(-0.840000000000000))*(npz)))); -evalcond[4]=x74; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst4; -gconst4=IKsign(((((IkReal(-100.000000000000))*((npy)*(npy))))+(((IkReal(-100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((npy)*(npy))))+(((IkReal(-1.00000000000000))*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x75=((gconst4)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x75))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x75))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x75)), ((IkReal(-49.0000000000000))*(npx)*(x75))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; + if (f <= 0.0f) + return 0.0f; + return sqrtf(f); } -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x76=IKcos(j6); -IkReal x77=IKsin(j6); -evalcond[0]=((((npx)*(x77)))+(((npy)*(x76)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x77)))+(((npx)*(x76)))+(((IkReal(-0.490000000000000))*(sj3)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -IkReal x234=((IkReal(1.00000000000000))*(pp)); -IkReal x235=x70; -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x234)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=x235; -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(-0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x234)))); -evalcond[4]=x235; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst5; -gconst5=IKsign(((((IkReal(100.000000000000))*((npy)*(npy))))+(((IkReal(100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) +inline double IKsqrt(double f) { -continue; - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x236=((gconst5)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x236))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x236))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x236)), ((IkReal(-49.0000000000000))*(npx)*(x236))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; + if (f <= 0.0) + return 0.0; + return sqrt(f); } -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) +inline float IKatan2(float fy, float fx) { -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x237=IKcos(j6); -IkReal x238=IKsin(j6); -evalcond[0]=((((npx)*(x238)))+(((npy)*(x237)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x238)))+(((npx)*(x237)))+(((IkReal(0.490000000000000))*(sj3)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - + if (std::isnan(fy)) + { + IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned + return float(IKPI_2); + } + else if (std::isnan(fx)) + { + return 0; + } + return atan2f(fy, fx); +} +inline double IKatan2(double fy, double fx) +{ + if (std::isnan(fy)) + { + IKFAST_ASSERT(!std::isnan(fx)); // if both are nan, probably wrong value will be returned + return IKPI_2; + } + else if (std::isnan(fx)) + { + return 0; + } + return atan2(fy, fx); } -} else -{ -if( 1 ) -{ -continue; - -} else +inline float IKsign(float f) { -} -} -} + if (f > 0) + { + return float(1); + } + else if (f < 0) + { + return float(-1); + } + return 0; } -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x239=(sj4)*(sj4); -IkReal x240=((IkReal(49.0000000000000))*(sj3)*(x239)); -IkReal x241=((IkReal(49.0000000000000))*(cj4)*(sj3)*(sj4)); -if( IKabs(((gconst3)*(((((npy)*(x241)))+(((npx)*(x240))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(npx)*(x241)))+(((npy)*(x240))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst3)*(((((npy)*(x241)))+(((npx)*(x240)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(npx)*(x241)))+(((npy)*(x240))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x242=IKcos(j6); -IkReal x243=IKsin(j6); -IkReal x244=((cj4)*(npy)); -IkReal x245=((IkReal(0.490000000000000))*(sj3)); -IkReal x246=((npy)*(sj4)); -IkReal x247=((IkReal(1.00000000000000))*(x243)); -IkReal x248=((npx)*(x243)); -IkReal x249=((npx)*(x242)); -evalcond[0]=((x248)+(((npy)*(x242)))+(((sj4)*(x245)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x247)))+(((IkReal(-1.00000000000000))*(cj4)*(x245)))+(x249)); -evalcond[2]=((((cj4)*(x248)))+(((x242)*(x244)))+(((sj4)*(x249)))+(((IkReal(-1.00000000000000))*(x246)*(x247)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(npx)*(sj4)*(x247)))+(((cj4)*(x249)))+(((IkReal(-1.00000000000000))*(x245)))+(((IkReal(-1.00000000000000))*(x244)*(x247)))+(((IkReal(-1.00000000000000))*(x242)*(x246)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +inline double IKsign(double f) { -continue; -} -} - -rotationfunction0(solutions); -} -} - + if (f > 0) + { + return 1.0; + } + else if (f < 0) + { + return -1.0; + } + return 0; } +/// 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, x43, x44, x45, x46, x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, + x58, x59, x60, x61, x62, x63; + x0 = IKcos(j[0]); + x1 = IKcos(j[3]); + x2 = IKcos(j[1]); + x3 = IKcos(j[2]); + x4 = IKsin(j[1]); + x5 = IKsin(j[3]); + x6 = IKsin(j[0]); + x7 = IKsin(j[2]); + x8 = IKsin(j[4]); + x9 = IKcos(j[4]); + x10 = IKcos(j[6]); + x11 = IKsin(j[6]); + x12 = IKsin(j[5]); + x13 = IKcos(j[5]); + x14 = ((IkReal(1.00000000000000)) * (x7)); + x15 = ((IkReal(1.00000000000000)) * (x1)); + x16 = ((IkReal(0.180000000000000)) * (x9)); + x17 = ((IkReal(0.420000000000000)) * (x2)); + x18 = ((IkReal(1.00000000000000)) * (x9)); + x19 = ((IkReal(1.00000000000000)) * (x2)); + x20 = ((IkReal(0.180000000000000)) * (x1)); + x21 = ((IkReal(1.00000000000000)) * (x13)); + x22 = ((IkReal(1.00000000000000)) * (x8)); + x23 = ((IkReal(0.180000000000000)) * (x5)); + x24 = ((IkReal(1.00000000000000)) * (x5)); + x25 = ((IkReal(0.420000000000000)) * (x1)); + x26 = ((IkReal(1.00000000000000)) * (x12)); + x27 = ((x0) * (x7)); + x28 = ((IkReal(-1.00000000000000)) * (x13)); + x29 = ((x3) * (x6)); + x30 = ((x0) * (x4)); + x31 = ((x4) * (x8)); + x32 = ((x4) * (x6)); + x33 = ((x6) * (x7)); + x34 = ((x0) * (x3)); + x35 = ((x3) * (x4)); + x36 = ((IkReal(-1.00000000000000)) * (x12)); + x37 = ((x14) * (x6)); + x38 = ((x15) * (x35)); + x39 = ((x14) * (x4) * (x9)); + x40 = ((x14) * (x31)); + x41 = ((((IkReal(-1.00000000000000)) * (x37))) + (((x2) * (x34)))); + x42 = ((((x2) * (x27))) + (x29)); + x43 = ((((x2) * (x29))) + (x27)); + x44 = ((((IkReal(-1.00000000000000)) * (x34))) + (((x2) * (x33)))); + x45 = ((((IkReal(-1.00000000000000)) * (x38))) + (((x2) * (x5)))); + x46 = ((((x1) * (x2))) + (((x35) * (x5)))); + x47 = ((((IkReal(-1.00000000000000)) * (x19) * (x34))) + (x37)); + x48 = ((((IkReal(-1.00000000000000)) * (x19) * (x5))) + (x38)); + x49 = ((((IkReal(-1.00000000000000)) * (x0) * (x14))) + + (((IkReal(-1.00000000000000)) * (x19) * (x29)))); + x50 = ((x42) * (x8)); + x51 = ((x1) * (x41)); + x52 = ((x44) * (x8)); + x53 = ((x49) * (x5)); + x54 = ((((x30) * (x5))) + (x51)); + x55 = ((((x32) * (x5))) + (((x1) * (x43)))); + x56 = ((((x47) * (x5))) + (((x1) * (x30)))); + x57 = ((((IkReal(-1.00000000000000)) * (x15) * (x43))) + + (((IkReal(-1.00000000000000)) * (x24) * (x32)))); + x58 = ((x53) + (((x1) * (x32)))); + x59 = ((x54) * (x9)); + x60 = ((x59) + (x50)); + x61 = ((x52) + (((x55) * (x9)))); + x62 = ((((x21) * (((((IkReal(-1.00000000000000)) * (x45) * (x9))) + (x40))))) + + (((IkReal(-1.00000000000000)) * (x26) * (x46)))); + x63 = ((x13) * (x60)); + eerot[0] = + ((((x10) * (((((x36) * (x56))) + (((x28) * (x60))))))) + + (((IkReal(-1.00000000000000)) * (x11) * + (((((x18) * (x42))) + (((x22) * (((((IkReal(-1.00000000000000)) * (x15) * (x41))) + + (((IkReal(-1.00000000000000)) * (x24) * (x30)))))))))))); + eerot[1] = + ((((x10) * + (((((x42) * (x9))) + (((x8) * (((((IkReal(-1.00000000000000)) * (x51))) + + (((IkReal(-1.00000000000000)) * (x30) * (x5))))))))))) + + (((x11) * (((((IkReal(-1.00000000000000)) * (x26) * (x56))) + + (((IkReal(-1.00000000000000)) * (x21) * (x60)))))))); + eerot[2] = ((((x12) * (((((IkReal(-1.00000000000000)) * (x18) * (x54))) + + (((IkReal(-1.00000000000000)) * (x22) * (x42))))))) + + (((x13) * (x56)))); + eetrans[0] = ((((x12) * (((((IkReal(-0.180000000000000)) * (x50))) + + (((IkReal(-1.00000000000000)) * (x16) * (x54))))))) + + (((IkReal(0.490000000000000)) * (x30))) + (((x25) * (x30))) + + (((x5) * (((((IkReal(0.420000000000000)) * (x33))) + + (((IkReal(-1.00000000000000)) * (x17) * (x34))))))) + + (((x13) * (((((x20) * (x30))) + (((x23) * (x47)))))))); + eerot[3] = ((((x11) * (((((IkReal(-1.00000000000000)) * (x18) * (x44))) + + (((IkReal(-1.00000000000000)) * (x22) * (x57))))))) + + (((x10) * (((((x36) * (x58))) + (((x28) * (x61)))))))); + eerot[4] = ((((x10) * (((((x57) * (x8))) + (((x44) * (x9))))))) + + (((x11) * (((((IkReal(-1.00000000000000)) * (x26) * (x58))) + + (((IkReal(-1.00000000000000)) * (x21) * (x61)))))))); + eerot[5] = ((((x13) * (x58))) + (((x12) * (((((IkReal(-1.00000000000000)) * (x18) * (x55))) + + (((IkReal(-1.00000000000000)) * (x22) * (x44)))))))); + eetrans[1] = ((((x5) * (((((IkReal(-1.00000000000000)) * (x17) * (x29))) + + (((IkReal(-0.420000000000000)) * (x27))))))) + + (((x12) * (((((IkReal(-0.180000000000000)) * (x52))) + + (((IkReal(-1.00000000000000)) * (x16) * (x55))))))) + + (((IkReal(0.490000000000000)) * (x32))) + (((x25) * (x32))) + + (((x13) * (((((x20) * (x32))) + (((x23) * (x49)))))))); + eerot[6] = + ((((x11) * (((((IkReal(-1.00000000000000)) * (x22) * (x48))) + (x39))))) + (((x10) * (x62)))); + eerot[7] = ((((x11) * (x62))) + + (((x10) * (((((IkReal(-1.00000000000000)) * (x39))) + (((x48) * (x8)))))))); + eerot[8] = + ((((x12) * (((((IkReal(-1.00000000000000)) * (x18) * (x45))) + (x40))))) + (((x13) * (x46)))); + eetrans[2] = + ((IkReal(0.410000000000000)) + (((x1) * (x17))) + + (((x13) * (((((x23) * (x35))) + (((x2) * (x20))))))) + + (((x12) * (((((IkReal(0.180000000000000)) * (x31) * (x7))) + + (((IkReal(-1.00000000000000)) * (x16) * (x45))))))) + + (((IkReal(0.420000000000000)) * (x35) * (x5))) + (((IkReal(0.490000000000000)) * (x2)))); } -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x250=((IkReal(49.0000000000000))*(cj4)*(sj3)); -IkReal x251=((IkReal(49.0000000000000))*(sj3)*(sj4)); -if( IKabs(((gconst2)*(((((npy)*(x250)))+(((npx)*(x251))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((npy)*(x251)))+(((IkReal(-1.00000000000000))*(npx)*(x250))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst2)*(((((npy)*(x250)))+(((npx)*(x251)))))), ((gconst2)*(((((npy)*(x251)))+(((IkReal(-1.00000000000000))*(npx)*(x250))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x252=IKcos(j6); -IkReal x253=IKsin(j6); -IkReal x254=((cj4)*(npy)); -IkReal x255=((IkReal(0.490000000000000))*(sj3)); -IkReal x256=((npy)*(sj4)); -IkReal x257=((IkReal(1.00000000000000))*(x253)); -IkReal x258=((npx)*(x253)); -IkReal x259=((npx)*(x252)); -evalcond[0]=((((sj4)*(x255)))+(((npy)*(x252)))+(x258)); -evalcond[1]=((((IkReal(-1.00000000000000))*(npy)*(x257)))+(((IkReal(-1.00000000000000))*(cj4)*(x255)))+(x259)); -evalcond[2]=((((sj4)*(x259)))+(((IkReal(-1.00000000000000))*(x256)*(x257)))+(((x252)*(x254)))+(((cj4)*(x258)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x252)*(x256)))+(((IkReal(-1.00000000000000))*(npx)*(sj4)*(x257)))+(((IkReal(-1.00000000000000))*(x254)*(x257)))+(((cj4)*(x259)))+(((IkReal(-1.00000000000000))*(x255)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) +IKFAST_API int GetNumFreeParameters() { return 1; } +IKFAST_API int* GetFreeParameters() { -continue; -} -} - -rotationfunction0(solutions); -} + static int freeparams[] = {4}; + return freeparams; } +IKFAST_API int GetNumJoints() { return 7; } -} +IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } -} +IKFAST_API int GetIkType() { return 0x67000001; } -} else -{ -IkReal x260=((IkReal(1.00000000000000))*(pp)); -IkReal x261=((IkReal(0.490000000000000))*(cj3)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j5)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x260)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=((IkReal(0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(x261)); -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x260)))); -evalcond[4]=((IkReal(-0.420000000000000))+(npz)+(((IkReal(-1.00000000000000))*(x261)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst6; -gconst6=IKsign(((((IkReal(100.000000000000))*((npy)*(npy))))+(((IkReal(100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst7; -IkReal x262=((IkReal(100.000000000000))*(sj4)); -gconst7=IKsign(((((x262)*((npy)*(npy))))+(((x262)*((npx)*(npx)))))); -dummyeval[0]=((((sj4)*((npx)*(npx))))+(((sj4)*((npy)*(npy))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[5]; -IkReal x263=((IkReal(1.00000000000000))*(pp)); -IkReal x264=((IkReal(0.490000000000000))*(cj3)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x263)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=((IkReal(0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(x264)); -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x263)))); -evalcond[4]=((IkReal(-0.420000000000000))+(npz)+(((IkReal(-1.00000000000000))*(x264)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) +class IKSolver { -{ -IkReal dummyeval[1]; -IkReal gconst8; -gconst8=IKsign(((((IkReal(100.000000000000))*((npy)*(npy))))+(((IkReal(100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=(((npx)*(npx))+((npy)*(npy))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; +public: + IkReal j0, cj0, sj0, htj0, j1, cj1, sj1, htj1, j2, cj2, sj2, htj2, j3, cj3, sj3, htj3, j5, cj5, + sj5, htj5, j6, cj6, sj6, htj6, j4, cj4, sj4, htj4, 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, _ij5[2], _nj5, _ij6[2], + _nj6, _ij4[2], _nj4; + + 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; + j5 = numeric_limits::quiet_NaN(); + _ij5[0] = -1; + _ij5[1] = -1; + _nj5 = -1; + j6 = numeric_limits::quiet_NaN(); + _ij6[0] = -1; + _ij6[1] = -1; + _nj6 = -1; + _ij4[0] = -1; + _ij4[1] = -1; + _nj4 = 0; + for (int dummyiter = 0; dummyiter < 1; ++dummyiter) + { + solutions.Clear(); + j4 = pfree[0]; + cj4 = cos(pfree[0]); + sj4 = sin(pfree[0]); + 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 = ((IkReal(-1.00000000000000)) * (r00)); + new_r01 = r01; + new_r02 = ((IkReal(-1.00000000000000)) * (r02)); + new_px = ((((IkReal(-0.180000000000000)) * (r02))) + (px)); + new_r10 = ((IkReal(-1.00000000000000)) * (r10)); + new_r11 = r11; + new_r12 = ((IkReal(-1.00000000000000)) * (r12)); + new_py = ((((IkReal(-0.180000000000000)) * (r12))) + (py)); + new_r20 = ((IkReal(-1.00000000000000)) * (r20)); + new_r21 = r21; + new_r22 = ((IkReal(-1.00000000000000)) * (r22)); + new_pz = ((IkReal(-0.410000000000000)) + (pz) + (((IkReal(-0.180000000000000)) * (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; + 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 = ((((IkReal(-1.00000000000000)) * (py) * (r20))) + (((pz) * (r10)))); + rxp0_1 = ((((px) * (r20))) + (((IkReal(-1.00000000000000)) * (pz) * (r00)))); + rxp0_2 = ((((IkReal(-1.00000000000000)) * (px) * (r10))) + (((py) * (r00)))); + rxp1_0 = ((((IkReal(-1.00000000000000)) * (py) * (r21))) + (((pz) * (r11)))); + rxp1_1 = ((((px) * (r21))) + (((IkReal(-1.00000000000000)) * (pz) * (r01)))); + rxp1_2 = ((((IkReal(-1.00000000000000)) * (px) * (r11))) + (((py) * (r01)))); + rxp2_0 = ((((IkReal(-1.00000000000000)) * (py) * (r22))) + (((pz) * (r12)))); + rxp2_1 = ((((px) * (r22))) + (((IkReal(-1.00000000000000)) * (pz) * (r02)))); + rxp2_2 = ((((IkReal(-1.00000000000000)) * (px) * (r12))) + (((py) * (r02)))); + { + IkReal j3array[2], cj3array[2], sj3array[2]; + bool j3valid[2] = {false}; + _nj3 = 2; + cj3array[0] = ((IkReal(-1.01190476190476)) + (((IkReal(2.42954324586978)) * (pp)))); + 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 (std::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 dummyeval[1]; + dummyeval[0] = (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = + ((IkReal(1.00000000000000)) + (((IkReal(2.33333333333333)) * (cj3))) + + (((IkReal(1.36111111111111)) * ((cj4) * (cj4)) * ((sj3) * (sj3)))) + + (((IkReal(1.36111111111111)) * ((cj3) * (cj3))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j5array[2], cj5array[2], sj5array[2]; + bool j5valid[2] = {false}; + _nj5 = 2; + IkReal x64 = ((IkReal(-0.420000000000000)) + + (((IkReal(-0.490000000000000)) * (cj3)))); + if (IKabs(x64) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(0.490000000000000)) * (cj4) * (sj3))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x65 = + ((IkReal(1.00000000000000)) * + (IKatan2(x64, ((IkReal(0.490000000000000)) * (cj4) * (sj3))))); + if (((((x64) * (x64)) + (((IkReal(0.240100000000000)) * ((cj4) * (cj4)) * + ((sj3) * (sj3)))))) < (IkReal)-0.00001) + continue; + if ((((npz) * + (((IKabs(IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3)))))))) != 0) + ? ((IkReal)1 / + (IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3))))))))) + : (IkReal)1.0e30)))) < -1 - IKFAST_SINCOS_THRESH || + (((npz) * + (((IKabs(IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3)))))))) != 0) + ? ((IkReal)1 / + (IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3))))))))) + : (IkReal)1.0e30)))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x66 = IKasin( + ((npz) * + (((IKabs(IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3)))))))) != 0) + ? ((IkReal)1 / + (IKabs(IKsqrt((((x64) * (x64)) + + (((IkReal(0.240100000000000)) * + ((cj4) * (cj4)) * ((sj3) * (sj3))))))))) + : (IkReal)1.0e30)))); + j5array[0] = ((x66) + (((IkReal(-1.00000000000000)) * (x65)))); + sj5array[0] = IKsin(j5array[0]); + cj5array[0] = IKcos(j5array[0]); + j5array[1] = + ((IkReal(3.14159265358979)) + (((IkReal(-1.00000000000000)) * (x65))) + + (((IkReal(-1.00000000000000)) * (x66)))); + sj5array[1] = IKsin(j5array[1]); + cj5array[1] = IKcos(j5array[1]); + if (j5array[0] > IKPI) + { + j5array[0] -= IK2PI; + } + else if (j5array[0] < -IKPI) + { + j5array[0] += IK2PI; + } + j5valid[0] = true; + if (j5array[1] > IKPI) + { + j5array[1] -= IK2PI; + } + else if (j5array[1] < -IKPI) + { + j5array[1] += IK2PI; + } + j5valid[1] = true; + for (int ij5 = 0; ij5 < 2; ++ij5) + { + if (!j5valid[ij5]) + { + continue; + } + _ij5[0] = ij5; + _ij5[1] = -1; + for (int iij5 = ij5 + 1; iij5 < 2; ++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 dummyeval[1]; + IkReal gconst0; + IkReal x67 = ((IkReal(100.000000000000)) * (sj5)); + gconst0 = + IKsign(((((x67) * ((npx) * (npx)))) + (((x67) * ((npy) * (npy)))))); + dummyeval[0] = + ((((sj5) * ((npy) * (npy)))) + (((sj5) * ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst1; + IkReal x68 = ((IkReal(2100.00000000000)) * (sj5)); + gconst1 = IKsign( + ((((x68) * ((npx) * (npx)))) + (((x68) * ((npy) * (npy)))))); + dummyeval[0] = + ((((sj5) * ((npy) * (npy)))) + (((sj5) * ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[5]; + IkReal x69 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x70 = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + + (((IkReal(-0.490000000000000)) * (cj3)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j5)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x69))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = x70; + evalcond[3] = ((IkReal(0.0637000000000000)) + + (((IkReal(-0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x69)))); + evalcond[4] = x70; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst2; + gconst2 = IKsign( + ((((IkReal(-100.000000000000)) * ((npy) * (npy)))) + + (((IkReal(-100.000000000000)) * ((npx) * (npx)))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * ((npy) * (npy)))) + + (((IkReal(-1.00000000000000)) * ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst3; + IkReal x71 = ((IkReal(100.000000000000)) * (sj4)); + gconst3 = IKsign(((((IkReal(-1.00000000000000)) * + (x71) * ((npx) * (npx)))) + + (((IkReal(-1.00000000000000)) * + (x71) * ((npy) * (npy)))))); + IkReal x72 = ((IkReal(1.00000000000000)) * (sj4)); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * (x72) * + ((npx) * (npx)))) + + (((IkReal(-1.00000000000000)) * (x72) * + ((npy) * (npy))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[5]; + IkReal x73 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x74 = x70; + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x73))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = x74; + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-1.00000000000000)) * (x73))) + + (((IkReal(-0.840000000000000)) * (npz)))); + evalcond[4] = x74; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst4; + gconst4 = + IKsign(((((IkReal(-100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-1.00000000000000)) * + ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x75 = ((gconst4) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x75))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x75))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((IkReal(49.0000000000000)) * + (npy) * (x75)), + ((IkReal(-49.0000000000000)) * + (npx) * (x75))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x76 = IKcos(j6); + IkReal x77 = IKsin(j6); + evalcond[0] = ((((npx) * (x77))) + + (((npy) * (x76)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * + (npy) * (x77))) + + (((npx) * (x76))) + + (((IkReal(-0.490000000000000)) * + (sj3)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + IkReal x234 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x235 = x70; + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod( + ((IkReal(1.11022302462516e-16)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x234))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = x235; + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x234)))); + evalcond[4] = x235; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst5; + gconst5 = + IKsign(((((IkReal(100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = + (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], + sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x236 = ((gconst5) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x236))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x236))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((IkReal(49.0000000000000)) * + (npy) * (x236)), + ((IkReal(-49.0000000000000)) * + (npx) * (x236))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; + ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x237 = IKcos(j6); + IkReal x238 = IKsin(j6); + evalcond[0] = ((((npx) * (x238))) + + (((npy) * (x237)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * + (npy) * (x238))) + + (((npx) * (x237))) + + (((IkReal(0.490000000000000)) * + (sj3)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x239 = (sj4) * (sj4); + IkReal x240 = + ((IkReal(49.0000000000000)) * (sj3) * (x239)); + IkReal x241 = ((IkReal(49.0000000000000)) * (cj4) * + (sj3) * (sj4)); + if (IKabs(((gconst3) * (((((npy) * (x241))) + + (((npx) * (x240))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst3) * + (((((IkReal(-1.00000000000000)) * (npx) * + (x241))) + + (((npy) * (x240))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst3) * + (((((npy) * (x241))) + (((npx) * (x240)))))), + ((gconst3) * (((((IkReal(-1.00000000000000)) * + (npx) * (x241))) + + (((npy) * (x240))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x242 = IKcos(j6); + IkReal x243 = IKsin(j6); + IkReal x244 = ((cj4) * (npy)); + IkReal x245 = + ((IkReal(0.490000000000000)) * (sj3)); + IkReal x246 = ((npy) * (sj4)); + IkReal x247 = + ((IkReal(1.00000000000000)) * (x243)); + IkReal x248 = ((npx) * (x243)); + IkReal x249 = ((npx) * (x242)); + evalcond[0] = ((x248) + (((npy) * (x242))) + + (((sj4) * (x245)))); + evalcond[1] = ((((IkReal(-1.00000000000000)) * + (npy) * (x247))) + + (((IkReal(-1.00000000000000)) * + (cj4) * (x245))) + + (x249)); + evalcond[2] = + ((((cj4) * (x248))) + (((x242) * (x244))) + + (((sj4) * (x249))) + + (((IkReal(-1.00000000000000)) * (x246) * + (x247)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (npx) * + (sj4) * (x247))) + + (((cj4) * (x249))) + + (((IkReal(-1.00000000000000)) * (x245))) + + (((IkReal(-1.00000000000000)) * (x244) * + (x247))) + + (((IkReal(-1.00000000000000)) * (x242) * + (x246)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x250 = + ((IkReal(49.0000000000000)) * (cj4) * (sj3)); + IkReal x251 = + ((IkReal(49.0000000000000)) * (sj3) * (sj4)); + if (IKabs(((gconst2) * (((((npy) * (x250))) + + (((npx) * (x251))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst2) * (((((npy) * (x251))) + + (((IkReal(-1.00000000000000)) * + (npx) * (x250))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst2) * + (((((npy) * (x250))) + (((npx) * (x251)))))), + ((gconst2) * (((((npy) * (x251))) + + (((IkReal(-1.00000000000000)) * + (npx) * (x250))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x252 = IKcos(j6); + IkReal x253 = IKsin(j6); + IkReal x254 = ((cj4) * (npy)); + IkReal x255 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x256 = ((npy) * (sj4)); + IkReal x257 = ((IkReal(1.00000000000000)) * (x253)); + IkReal x258 = ((npx) * (x253)); + IkReal x259 = ((npx) * (x252)); + evalcond[0] = ((((sj4) * (x255))) + + (((npy) * (x252))) + (x258)); + evalcond[1] = ((((IkReal(-1.00000000000000)) * + (npy) * (x257))) + + (((IkReal(-1.00000000000000)) * + (cj4) * (x255))) + + (x259)); + evalcond[2] = + ((((sj4) * (x259))) + + (((IkReal(-1.00000000000000)) * (x256) * + (x257))) + + (((x252) * (x254))) + (((cj4) * (x258)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x252) * + (x256))) + + (((IkReal(-1.00000000000000)) * (npx) * (sj4) * + (x257))) + + (((IkReal(-1.00000000000000)) * (x254) * + (x257))) + + (((cj4) * (x259))) + + (((IkReal(-1.00000000000000)) * (x255)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + IkReal x260 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x261 = ((IkReal(0.490000000000000)) * (cj3)); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j5)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x260))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = + ((IkReal(0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + (x261)); + evalcond[3] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x260)))); + evalcond[4] = ((IkReal(-0.420000000000000)) + (npz) + + (((IkReal(-1.00000000000000)) * (x261)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst6; + gconst6 = IKsign( + ((((IkReal(100.000000000000)) * ((npy) * (npy)))) + + (((IkReal(100.000000000000)) * ((npx) * (npx)))))); + dummyeval[0] = (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst7; + IkReal x262 = ((IkReal(100.000000000000)) * (sj4)); + gconst7 = IKsign(((((x262) * ((npy) * (npy)))) + + (((x262) * ((npx) * (npx)))))); + dummyeval[0] = ((((sj4) * ((npx) * (npx)))) + + (((sj4) * ((npy) * (npy))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[5]; + IkReal x263 = ((IkReal(1.00000000000000)) * (pp)); + IkReal x264 = + ((IkReal(0.490000000000000)) * (cj3)); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x263))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = + ((IkReal(0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + + (x264)); + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x263)))); + evalcond[4] = + ((IkReal(-0.420000000000000)) + (npz) + + (((IkReal(-1.00000000000000)) * (x264)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst8; + gconst8 = + IKsign(((((IkReal(100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = + (((npx) * (npx)) + ((npy) * (npy))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], + sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x265 = ((gconst8) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x265))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x265))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((IkReal(49.0000000000000)) * + (npy) * (x265)), + ((IkReal(-49.0000000000000)) * + (npx) * (x265))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; + ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x266 = IKcos(j6); + IkReal x267 = IKsin(j6); + evalcond[0] = ((((npx) * (x267))) + + (((npy) * (x266)))); + evalcond[1] = + ((((npx) * (x266))) + + (((IkReal(-1.00000000000000)) * + (npy) * (x267))) + + (((IkReal(0.490000000000000)) * + (sj3)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + IkReal x268 = + ((IkReal(1.00000000000000)) * (pp)); + IkReal x269 = + ((IkReal(0.490000000000000)) * (cj3)); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + + (j4)), + IkReal(6.28318530717959)))); + evalcond[1] = + ((IkReal(0.416500000000000)) + + (((IkReal(-1.00000000000000)) * (x268))) + + (((IkReal(0.411600000000000)) * (cj3)))); + evalcond[2] = + ((IkReal(0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (npz))) + + (x269)); + evalcond[3] = + ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (npz))) + + (((IkReal(-1.00000000000000)) * (x268)))); + evalcond[4] = + ((IkReal(-0.420000000000000)) + (npz) + + (((IkReal(-1.00000000000000)) * (x269)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst9; + gconst9 = + IKsign(((((IkReal(-100.000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-100.000000000000)) * + ((npx) * (npx)))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * + ((npy) * (npy)))) + + (((IkReal(-1.00000000000000)) * + ((npx) * (npx))))); + if (IKabs(dummyeval[0]) < + 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j6array[1], cj6array[1], + sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x270 = ((gconst9) * (sj3)); + if (IKabs(((IkReal(49.0000000000000)) * + (npy) * (x270))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-49.0000000000000)) * + (npx) * (x270))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((IkReal(49.0000000000000)) * + (npy) * (x270)), + ((IkReal(-49.0000000000000)) * + (npx) * (x270))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; + ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - + cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - + sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x271 = IKcos(j6); + IkReal x272 = IKsin(j6); + evalcond[0] = ((((npy) * (x271))) + + (((npx) * (x272)))); + evalcond[1] = + ((((IkReal( + -0.490000000000000)) * + (sj3))) + + (((IkReal(-1.00000000000000)) * + (npy) * (x272))) + + (((npx) * (x271)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x273 = (sj4) * (sj4); + IkReal x274 = ((cj4) * (sj4)); + IkReal x275 = + ((IkReal(49.0000000000000)) * (npx) * (sj3)); + IkReal x276 = + ((IkReal(49.0000000000000)) * (npy) * (sj3)); + if (IKabs(((gconst7) * + (((((IkReal(-1.00000000000000)) * + (x273) * (x275))) + + (((x274) * (x276))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst7) * + (((((IkReal(-1.00000000000000)) * + (x273) * (x276))) + + (((IkReal(-1.00000000000000)) * + (x274) * (x275))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst7) * (((((IkReal(-1.00000000000000)) * + (x273) * (x275))) + + (((x274) * (x276)))))), + ((gconst7) * (((((IkReal(-1.00000000000000)) * + (x273) * (x276))) + + (((IkReal(-1.00000000000000)) * + (x274) * (x275))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x277 = IKcos(j6); + IkReal x278 = IKsin(j6); + IkReal x279 = + ((IkReal(1.00000000000000)) * (sj4)); + IkReal x280 = + ((IkReal(0.490000000000000)) * (sj3)); + IkReal x281 = ((npy) * (x277)); + IkReal x282 = ((npx) * (x277)); + IkReal x283 = ((npx) * (x278)); + IkReal x284 = ((npy) * (x278)); + evalcond[0] = + ((x283) + (x281) + (((sj4) * (x280)))); + evalcond[1] = + ((x282) + (((IkReal(-1.00000000000000)) * + (x284))) + + (((cj4) * (x280)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x279) * + (x282))) + + (((cj4) * (x283))) + (((cj4) * (x281))) + + (((sj4) * (x284)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * + (cj4) * (x282))) + + (((IkReal(-1.00000000000000)) * + (x280))) + + (((IkReal(-1.00000000000000)) * + (x279) * (x281))) + + (((IkReal(-1.00000000000000)) * + (x279) * (x283))) + + (((cj4) * (x284)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x285 = + ((IkReal(49.0000000000000)) * (cj4) * (sj3)); + IkReal x286 = + ((IkReal(49.0000000000000)) * (sj3) * (sj4)); + if (IKabs( + ((gconst6) * (((((IkReal(-1.00000000000000)) * + (npx) * (x286))) + + (((npy) * (x285))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst6) * + (((((IkReal(-1.00000000000000)) * (npy) * + (x286))) + + (((IkReal(-1.00000000000000)) * (npx) * + (x285))))))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst6) * (((((IkReal(-1.00000000000000)) * + (npx) * (x286))) + + (((npy) * (x285)))))), + ((gconst6) * (((((IkReal(-1.00000000000000)) * + (npy) * (x286))) + + (((IkReal(-1.00000000000000)) * + (npx) * (x285))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[4]; + IkReal x287 = IKcos(j6); + IkReal x288 = IKsin(j6); + IkReal x289 = + ((IkReal(1.00000000000000)) * (sj4)); + IkReal x290 = + ((IkReal(0.490000000000000)) * (sj3)); + IkReal x291 = ((npy) * (x287)); + IkReal x292 = ((npx) * (x287)); + IkReal x293 = ((npx) * (x288)); + IkReal x294 = ((npy) * (x288)); + evalcond[0] = + ((((sj4) * (x290))) + (x291) + (x293)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x294))) + + (((cj4) * (x290))) + (x292)); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (x289) * + (x292))) + + (((cj4) * (x291))) + (((cj4) * (x293))) + + (((sj4) * (x294)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (cj4) * + (x292))) + + (((IkReal(-1.00000000000000)) * (x289) * + (x293))) + + (((IkReal(-1.00000000000000)) * (x289) * + (x291))) + + (((IkReal(-1.00000000000000)) * (x290))) + + (((cj4) * (x294)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x295 = ((IkReal(2500.00000000000)) * (pp)); + IkReal x296 = ((IkReal(2100.00000000000)) * (cj5) * (npz)); + IkReal x297 = + ((IkReal(1029.00000000000)) * (sj3) * (sj4) * (sj5)); + if (IKabs(((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x295))) + + (((IkReal(-1.00000000000000)) * (npy) * (x296))) + + (((IkReal(-1.00000000000000)) * (npx) * (x297))) + + (((IkReal(159.250000000000)) * (npy))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x297))) + + (((npx) * (x295))) + (((npx) * (x296))) + + (((IkReal(-159.250000000000)) * (npx))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = IKatan2( + ((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x295))) + + (((IkReal(-1.00000000000000)) * (npy) * (x296))) + + (((IkReal(-1.00000000000000)) * (npx) * (x297))) + + (((IkReal(159.250000000000)) * (npy)))))), + ((gconst1) * + (((((IkReal(-1.00000000000000)) * (npy) * (x297))) + + (((npx) * (x295))) + (((npx) * (x296))) + + (((IkReal(-159.250000000000)) * (npx))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[6]; + IkReal x298 = IKcos(j6); + IkReal x299 = IKsin(j6); + IkReal x300 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x301 = ((cj5) * (npz)); + IkReal x302 = ((cj4) * (cj5)); + IkReal x303 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x304 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x305 = ((IkReal(0.840000000000000)) * (sj5)); + IkReal x306 = ((npz) * (sj5)); + IkReal x307 = ((npy) * (x298)); + IkReal x308 = ((npx) * (x298)); + IkReal x309 = ((npy) * (x299)); + IkReal x310 = ((npx) * (x299)); + evalcond[0] = ((x307) + (x310) + (((sj4) * (x303)))); + evalcond[1] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-1.00000000000000)) * (x305) * (x309))) + + (((x305) * (x308))) + + (((IkReal(-0.840000000000000)) * (x301)))); + evalcond[2] = + ((IkReal(-0.420000000000000)) + (((sj5) * (x308))) + + (((IkReal(-1.00000000000000)) * (x301))) + + (((IkReal(-1.00000000000000)) * (sj5) * (x309))) + + (((IkReal(-1.00000000000000)) * (x304)))); + evalcond[3] = + ((((IkReal(-0.420000000000000)) * (sj5))) + + (((IkReal(-1.00000000000000)) * (x302) * (x303))) + + (((IkReal(-1.00000000000000)) * (x309))) + (x308) + + (((IkReal(-1.00000000000000)) * (sj5) * (x304)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (cj5) * (x300) * + (x309))) + + (((cj5) * (sj4) * (x308))) + (((cj4) * (x310))) + + (((sj4) * (x306))) + (((cj4) * (x307)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x302) * (x309))) + + (((IkReal(-1.00000000000000)) * (x300) * (x310))) + + (((x302) * (x308))) + + (((IkReal(-1.00000000000000)) * (x300) * (x307))) + + (((cj4) * (x306))) + + (((IkReal(-1.00000000000000)) * (x303)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j6array[1], cj6array[1], sj6array[1]; + bool j6valid[1] = {false}; + _nj6 = 1; + IkReal x311 = ((IkReal(49.0000000000000)) * (npx)); + IkReal x312 = ((IkReal(49.0000000000000)) * (npy)); + IkReal x313 = ((sj3) * (sj4) * (sj5)); + IkReal x314 = ((IkReal(100.000000000000)) * (cj5) * (npz)); + if (IKabs(((gconst0) * + (((((IkReal(-1.00000000000000)) * (npy) * (x314))) + + (((IkReal(-1.00000000000000)) * (x311) * (x313))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x312))) + + (((IkReal(-42.0000000000000)) * (npy))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst0) * + (((((IkReal(-1.00000000000000)) * (x312) * (x313))) + + (((npx) * (x314))) + (((cj3) * (x311))) + + (((IkReal(42.0000000000000)) * (npx))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j6array[0] = + IKatan2(((gconst0) * + (((((IkReal(-1.00000000000000)) * (npy) * (x314))) + + (((IkReal(-1.00000000000000)) * (x311) * (x313))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x312))) + + (((IkReal(-42.0000000000000)) * (npy)))))), + ((gconst0) * + (((((IkReal(-1.00000000000000)) * (x312) * (x313))) + + (((npx) * (x314))) + (((cj3) * (x311))) + + (((IkReal(42.0000000000000)) * (npx))))))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + for (int ij6 = 0; ij6 < 1; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 1; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < + IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[6]; + IkReal x315 = IKcos(j6); + IkReal x316 = IKsin(j6); + IkReal x317 = ((IkReal(1.00000000000000)) * (sj4)); + IkReal x318 = ((cj5) * (npz)); + IkReal x319 = ((cj4) * (cj5)); + IkReal x320 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x321 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x322 = ((IkReal(0.840000000000000)) * (sj5)); + IkReal x323 = ((npz) * (sj5)); + IkReal x324 = ((npy) * (x315)); + IkReal x325 = ((npx) * (x315)); + IkReal x326 = ((npy) * (x316)); + IkReal x327 = ((npx) * (x316)); + evalcond[0] = ((((sj4) * (x320))) + (x324) + (x327)); + evalcond[1] = + ((IkReal(0.0637000000000000)) + (((x322) * (x325))) + + (((IkReal(-0.840000000000000)) * (x318))) + + (((IkReal(-1.00000000000000)) * (x322) * (x326))) + + (((IkReal(-1.00000000000000)) * (pp)))); + evalcond[2] = + ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x321))) + + (((sj5) * (x325))) + + (((IkReal(-1.00000000000000)) * (sj5) * (x326))) + + (((IkReal(-1.00000000000000)) * (x318)))); + evalcond[3] = + ((((IkReal(-0.420000000000000)) * (sj5))) + + (((IkReal(-1.00000000000000)) * (x326))) + (x325) + + (((IkReal(-1.00000000000000)) * (x319) * (x320))) + + (((IkReal(-1.00000000000000)) * (sj5) * (x321)))); + evalcond[4] = + ((((cj5) * (sj4) * (x325))) + (((sj4) * (x323))) + + (((IkReal(-1.00000000000000)) * (cj5) * (x317) * (x326))) + + (((cj4) * (x324))) + (((cj4) * (x327)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x317) * (x327))) + + (((IkReal(-1.00000000000000)) * (x317) * (x324))) + + (((IkReal(-1.00000000000000)) * (x320))) + + (((cj4) * (x323))) + + (((IkReal(-1.00000000000000)) * (x319) * (x326))) + + (((x319) * (x325)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j6array[2], cj6array[2], sj6array[2]; + bool j6valid[2] = {false}; + _nj6 = 2; + if (IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x328 = ((IkReal(1.00000000000000)) * (IKatan2(npy, npx))); + if (((((npx) * (npx)) + ((npy) * (npy)))) < (IkReal)-0.00001) + continue; + if ((((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) < -1 - IKFAST_SINCOS_THRESH || + (((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x329 = IKasin( + ((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))); + j6array[0] = ((((IkReal(-1.00000000000000)) * (x328))) + + (((IkReal(-1.00000000000000)) * (x329)))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + j6array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x328))) + (x329)); + sj6array[1] = IKsin(j6array[1]); + cj6array[1] = IKcos(j6array[1]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + if (j6array[1] > IKPI) + { + j6array[1] -= IK2PI; + } + else if (j6array[1] < -IKPI) + { + j6array[1] += IK2PI; + } + j6valid[1] = true; + for (int ij6 = 0; ij6 < 2; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 2; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + { + IkReal evalcond[2]; + IkReal x330 = (npx) * (npx); + IkReal x331 = (cj4) * (cj4); + IkReal x332 = (sj4) * (sj4); + IkReal x333 = IKcos(j6); + IkReal x334 = (npy) * (npy); + IkReal x335 = IKsin(j6); + IkReal x336 = ((npx) * (npy)); + IkReal x337 = ((IkReal(0.490000000000000)) * (sj3) * (sj4)); + IkReal x338 = ((IkReal(1.00000000000000)) * (x334)); + evalcond[0] = ((((x333) * (((((x331) * (x336))) + (((x332) * (x336))))))) + + (((x335) * (((((x330) * (x331))) + (((x330) * (x332))))))) + + (((npx) * (x337)))); + evalcond[1] = + ((((x335) * (((((IkReal(-1.00000000000000)) * (x332) * (x336))) + + (((IkReal(-1.00000000000000)) * (x331) * (x336))))))) + + (((x333) * (((((IkReal(-1.00000000000000)) * (x331) * (x338))) + + (((IkReal(-1.00000000000000)) * (x332) * (x338))))))) + + (((IkReal(-1.00000000000000)) * (npy) * (x337)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst10; + IkReal x339 = ((cj6) * (npx)); + IkReal x340 = ((IkReal(49.0000000000000)) * (cj3)); + IkReal x341 = ((npy) * (sj6)); + gconst10 = + IKsign(((((IkReal(-42.0000000000000)) * (x339))) + + (((IkReal(49.0000000000000)) * (cj4) * (npz) * (sj3))) + + (((x340) * (x341))) + (((IkReal(42.0000000000000)) * (x341))) + + (((IkReal(-1.00000000000000)) * (x339) * (x340))))); + IkReal x342 = ((npy) * (sj6)); + IkReal x343 = ((cj6) * (npx)); + IkReal x344 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = + ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + (x342) + + (((IkReal(-1.00000000000000)) * (x343) * (x344))) + + (((IkReal(-1.00000000000000)) * (x343))) + (((x342) * (x344)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst11; + IkReal x345 = ((npy) * (sj6)); + IkReal x346 = ((IkReal(1029.00000000000)) * (cj3)); + IkReal x347 = ((cj6) * (npx)); + gconst11 = + IKsign(((((x345) * (x346))) + + (((IkReal(1029.00000000000)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x346) * (x347))) + + (((IkReal(-882.000000000000)) * (x347))) + + (((IkReal(882.000000000000)) * (x345))))); + IkReal x348 = ((npy) * (sj6)); + IkReal x349 = ((cj6) * (npx)); + IkReal x350 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = + ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x349) * (x350))) + (x348) + + (((x348) * (x350))) + (((IkReal(-1.00000000000000)) * (x349)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x351 = ((cj4) * (sj3)); + IkReal x352 = ((IkReal(1225.00000000000)) * (pp)); + IkReal x353 = ((IkReal(2100.00000000000)) * (npz)); + if (IKabs(((gconst11) * + (((IkReal(66.8850000000000)) + + (((IkReal(-1.00000000000000)) * (cj3) * (x352))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(78.0325000000000)) * (cj3))) + + (((npz) * (x353))))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst11) * + (((((IkReal(78.0325000000000)) * (x351))) + + (((cj6) * (npx) * (x353))) + + (((IkReal(-1.00000000000000)) * (x351) * (x352))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * + (x353))))))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst11) * + (((IkReal(66.8850000000000)) + + (((IkReal(-1.00000000000000)) * (cj3) * (x352))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(78.0325000000000)) * (cj3))) + + (((npz) * (x353)))))), + ((gconst11) * + (((((IkReal(78.0325000000000)) * (x351))) + + (((cj6) * (npx) * (x353))) + + (((IkReal(-1.00000000000000)) * (x351) * (x352))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (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[6]; + IkReal x354 = IKcos(j5); + IkReal x355 = IKsin(j5); + IkReal x356 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x357 = ((cj6) * (sj4)); + IkReal x358 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x359 = ((sj4) * (sj6)); + IkReal x360 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x361 = ((cj6) * (npx)); + IkReal x362 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x363 = ((npz) * (x355)); + IkReal x364 = ((cj4) * (x354)); + IkReal x365 = ((sj6) * (x355)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x354) * (x362))) + + (((IkReal(-0.420000000000000)) * (x354))) + + (((IkReal(-1.00000000000000)) * (x360))) + + (((cj4) * (x355) * (x358)))); + evalcond[1] = + ((IkReal(0.0637000000000000)) + + (((IkReal(-0.840000000000000)) * (npz) * (x354))) + + (((IkReal(0.840000000000000)) * (x355) * (x361))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x365)))); + evalcond[2] = + ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x354) * (x360))) + + (((IkReal(-1.00000000000000)) * (x362))) + + (((x355) * (x361))) + + (((IkReal(-1.00000000000000)) * (x356) * (x365)))); + evalcond[3] = + ((((IkReal(-0.420000000000000)) * (x355))) + + (((IkReal(-1.00000000000000)) * (x358) * (x364))) + + (((IkReal(-1.00000000000000)) * (x355) * (x362))) + + (x361) + (((IkReal(-1.00000000000000)) * (sj6) * (x356)))); + evalcond[4] = + ((((sj4) * (x363))) + (((npx) * (x354) * (x357))) + + (((cj4) * (npx) * (sj6))) + (((cj4) * (cj6) * (npy))) + + (((IkReal(-1.00000000000000)) * (x354) * (x356) * + (x359)))); + evalcond[5] = + ((((x361) * (x364))) + + (((IkReal(-1.00000000000000)) * (x358))) + + (((IkReal(-1.00000000000000)) * (x356) * (x357))) + + (((cj4) * (x363))) + + (((IkReal(-1.00000000000000)) * (npx) * (x359))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x356) * (x364)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x366 = ((cj4) * (sj3)); + IkReal x367 = ((IkReal(100.000000000000)) * (npz)); + if (IKabs(( + (gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x367))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3)))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(( + (gconst10) * + (((((IkReal(-20.5800000000000)) * (x366))) + + (((cj6) * (npx) * (x367))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x366))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x367))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x367))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3))))))), + ((gconst10) * + (((((IkReal(-20.5800000000000)) * (x366))) + + (((cj6) * (npx) * (x367))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x366))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x367))))))); + 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 x368 = IKcos(j5); + IkReal x369 = IKsin(j5); + IkReal x370 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x371 = ((cj6) * (sj4)); + IkReal x372 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x373 = ((sj4) * (sj6)); + IkReal x374 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x375 = ((cj6) * (npx)); + IkReal x376 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x377 = ((npz) * (x369)); + IkReal x378 = ((cj4) * (x368)); + IkReal x379 = ((sj6) * (x369)); + evalcond[0] = ((((IkReal(-1.00000000000000)) * (x368) * (x376))) + + (((cj4) * (x369) * (x372))) + + (((IkReal(-1.00000000000000)) * (x374))) + + (((IkReal(-0.420000000000000)) * (x368)))); + evalcond[1] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (x369) * (x375))) + + (((IkReal(-0.840000000000000)) * (npz) * (x368))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x379)))); + evalcond[2] = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x368) * (x374))) + + (((IkReal(-1.00000000000000)) * (x370) * (x379))) + + (((x369) * (x375))) + + (((IkReal(-1.00000000000000)) * (x376)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (x372) * (x378))) + + (x375) + (((IkReal(-0.420000000000000)) * (x369))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x370))) + + (((IkReal(-1.00000000000000)) * (x369) * (x376)))); + evalcond[4] = + ((((sj4) * (x377))) + + (((IkReal(-1.00000000000000)) * (x368) * (x370) * (x373))) + + (((cj4) * (npx) * (sj6))) + (((cj4) * (cj6) * (npy))) + + (((npx) * (x368) * (x371)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (npx) * (x373))) + + (((x375) * (x378))) + + (((IkReal(-1.00000000000000)) * (x370) * (x371))) + + (((IkReal(-1.00000000000000)) * (x372))) + (((cj4) * (x377))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x370) * (x378)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j6array[2], cj6array[2], sj6array[2]; + bool j6valid[2] = {false}; + _nj6 = 2; + if (IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x380 = ((IkReal(1.00000000000000)) * (IKatan2(npy, npx))); + if (((((npx) * (npx)) + ((npy) * (npy)))) < (IkReal)-0.00001) + continue; + if ((((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) < -1 - IKFAST_SINCOS_THRESH || + (((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))) > 1 + IKFAST_SINCOS_THRESH) + continue; + IkReal x381 = IKasin( + ((IkReal(0.490000000000000)) * (sj3) * (sj4) * + (((IKabs(IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy)))))) != 0) + ? ((IkReal)1 / (IKabs(IKsqrt((((npx) * (npx)) + ((npy) * (npy))))))) + : (IkReal)1.0e30)))); + j6array[0] = ((((IkReal(-1.00000000000000)) * (x381))) + + (((IkReal(-1.00000000000000)) * (x380)))); + sj6array[0] = IKsin(j6array[0]); + cj6array[0] = IKcos(j6array[0]); + j6array[1] = ((IkReal(3.14159265358979)) + (x381) + + (((IkReal(-1.00000000000000)) * (x380)))); + sj6array[1] = IKsin(j6array[1]); + cj6array[1] = IKcos(j6array[1]); + if (j6array[0] > IKPI) + { + j6array[0] -= IK2PI; + } + else if (j6array[0] < -IKPI) + { + j6array[0] += IK2PI; + } + j6valid[0] = true; + if (j6array[1] > IKPI) + { + j6array[1] -= IK2PI; + } + else if (j6array[1] < -IKPI) + { + j6array[1] += IK2PI; + } + j6valid[1] = true; + for (int ij6 = 0; ij6 < 2; ++ij6) + { + if (!j6valid[ij6]) + { + continue; + } + _ij6[0] = ij6; + _ij6[1] = -1; + for (int iij6 = ij6 + 1; iij6 < 2; ++iij6) + { + if (j6valid[iij6] && + IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH && + IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH) + { + j6valid[iij6] = false; + _ij6[1] = iij6; + break; + } + } + j6 = j6array[ij6]; + cj6 = cj6array[ij6]; + sj6 = sj6array[ij6]; + + { + IkReal dummyeval[1]; + IkReal gconst10; + IkReal x382 = ((cj6) * (npx)); + IkReal x383 = ((IkReal(49.0000000000000)) * (cj3)); + IkReal x384 = ((npy) * (sj6)); + gconst10 = + IKsign(((((IkReal(-42.0000000000000)) * (x382))) + + (((IkReal(42.0000000000000)) * (x384))) + (((x383) * (x384))) + + (((IkReal(49.0000000000000)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x382) * (x383))))); + IkReal x385 = ((npy) * (sj6)); + IkReal x386 = ((cj6) * (npx)); + IkReal x387 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = + ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + (x385) + + (((IkReal(-1.00000000000000)) * (x386))) + + (((IkReal(-1.00000000000000)) * (x386) * (x387))) + (((x385) * (x387)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst11; + IkReal x388 = ((npy) * (sj6)); + IkReal x389 = ((IkReal(1029.00000000000)) * (cj3)); + IkReal x390 = ((cj6) * (npx)); + gconst11 = + IKsign(((((IkReal(-1.00000000000000)) * (x389) * (x390))) + + (((IkReal(-882.000000000000)) * (x390))) + (((x388) * (x389))) + + (((IkReal(1029.00000000000)) * (cj4) * (npz) * (sj3))) + + (((IkReal(882.000000000000)) * (x388))))); + IkReal x391 = ((npy) * (sj6)); + IkReal x392 = ((cj6) * (npx)); + IkReal x393 = ((IkReal(1.16666666666667)) * (cj3)); + dummyeval[0] = ((((IkReal(1.16666666666667)) * (cj4) * (npz) * (sj3))) + + (((IkReal(-1.00000000000000)) * (x392))) + (x391) + + (((x391) * (x393))) + + (((IkReal(-1.00000000000000)) * (x392) * (x393)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x394 = ((cj4) * (sj3)); + IkReal x395 = ((IkReal(1225.00000000000)) * (pp)); + IkReal x396 = ((IkReal(2100.00000000000)) * (npz)); + if (IKabs(((gconst11) * + (((IkReal(66.8850000000000)) + (((npz) * (x396))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x395))) + + (((IkReal(78.0325000000000)) * (cj3))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(( + (gconst11) * + (((((cj6) * (npx) * (x396))) + + (((IkReal(-1.00000000000000)) * (x394) * (x395))) + + (((IkReal(78.0325000000000)) * (x394))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x396))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst11) * (((IkReal(66.8850000000000)) + (((npz) * (x396))) + + (((IkReal(-1050.00000000000)) * (pp))) + + (((IkReal(-1.00000000000000)) * (cj3) * (x395))) + + (((IkReal(78.0325000000000)) * (cj3)))))), + ((gconst11) * + (((((cj6) * (npx) * (x396))) + + (((IkReal(-1.00000000000000)) * (x394) * (x395))) + + (((IkReal(78.0325000000000)) * (x394))) + + (((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x396))))))); + 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 x397 = IKcos(j5); + IkReal x398 = IKsin(j5); + IkReal x399 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x400 = ((cj6) * (sj4)); + IkReal x401 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x402 = ((sj4) * (sj6)); + IkReal x403 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x404 = ((cj6) * (npx)); + IkReal x405 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x406 = ((npz) * (x398)); + IkReal x407 = ((cj4) * (x397)); + IkReal x408 = ((sj6) * (x398)); + evalcond[0] = ((((cj4) * (x398) * (x401))) + + (((IkReal(-1.00000000000000)) * (x397) * (x405))) + + (((IkReal(-0.420000000000000)) * (x397))) + + (((IkReal(-1.00000000000000)) * (x403)))); + evalcond[1] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (x398) * (x404))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x408))) + + (((IkReal(-0.840000000000000)) * (npz) * (x397)))); + evalcond[2] = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x399) * (x408))) + + (((IkReal(-1.00000000000000)) * (x397) * (x403))) + + (((x398) * (x404))) + + (((IkReal(-1.00000000000000)) * (x405)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x401) * (x407))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x399))) + (x404) + + (((IkReal(-1.00000000000000)) * (x398) * (x405))) + + (((IkReal(-0.420000000000000)) * (x398)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x397) * (x399) * (x402))) + + (((npx) * (x397) * (x400))) + (((cj4) * (npx) * (sj6))) + + (((cj4) * (cj6) * (npy))) + (((sj4) * (x406)))); + evalcond[5] = + ((((x404) * (x407))) + + (((IkReal(-1.00000000000000)) * (sj6) * (x399) * (x407))) + + (((cj4) * (x406))) + + (((IkReal(-1.00000000000000)) * (x399) * (x400))) + + (((IkReal(-1.00000000000000)) * (npx) * (x402))) + + (((IkReal(-1.00000000000000)) * (x401)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + else + { + { + IkReal j5array[1], cj5array[1], sj5array[1]; + bool j5valid[1] = {false}; + _nj5 = 1; + IkReal x409 = ((cj4) * (sj3)); + IkReal x410 = ((IkReal(100.000000000000)) * (npz)); + if (IKabs(((gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x410))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3)))))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst10) * + (((((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x410))) + + (((cj6) * (npx) * (x410))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x409))) + + (((IkReal(-20.5800000000000)) * (x409))))))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j5array[0] = IKatan2( + ((gconst10) * + (((IkReal(-17.6400000000000)) + + (((IkReal(-41.1600000000000)) * (cj3))) + (((npz) * (x410))) + + (((IkReal(-24.0100000000000)) * ((cj3) * (cj3))))))), + ((gconst10) * + (((((IkReal(-1.00000000000000)) * (npy) * (sj6) * (x410))) + + (((cj6) * (npx) * (x410))) + + (((IkReal(-24.0100000000000)) * (cj3) * (x409))) + + (((IkReal(-20.5800000000000)) * (x409))))))); + 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 x411 = IKcos(j5); + IkReal x412 = IKsin(j5); + IkReal x413 = ((IkReal(1.00000000000000)) * (npy)); + IkReal x414 = ((cj6) * (sj4)); + IkReal x415 = ((IkReal(0.490000000000000)) * (sj3)); + IkReal x416 = ((sj4) * (sj6)); + IkReal x417 = ((IkReal(1.00000000000000)) * (npz)); + IkReal x418 = ((cj6) * (npx)); + IkReal x419 = ((IkReal(0.490000000000000)) * (cj3)); + IkReal x420 = ((npz) * (x412)); + IkReal x421 = ((cj4) * (x411)); + IkReal x422 = ((sj6) * (x412)); + evalcond[0] = ((((cj4) * (x412) * (x415))) + + (((IkReal(-0.420000000000000)) * (x411))) + + (((IkReal(-1.00000000000000)) * (x411) * (x419))) + + (((IkReal(-1.00000000000000)) * (x417)))); + evalcond[1] = ((IkReal(0.0637000000000000)) + + (((IkReal(0.840000000000000)) * (x412) * (x418))) + + (((IkReal(-1.00000000000000)) * (pp))) + + (((IkReal(-0.840000000000000)) * (npy) * (x422))) + + (((IkReal(-0.840000000000000)) * (npz) * (x411)))); + evalcond[2] = ((IkReal(-0.420000000000000)) + + (((IkReal(-1.00000000000000)) * (x411) * (x417))) + + (((x412) * (x418))) + + (((IkReal(-1.00000000000000)) * (x413) * (x422))) + + (((IkReal(-1.00000000000000)) * (x419)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (sj6) * (x413))) + + (((IkReal(-1.00000000000000)) * (x412) * (x419))) + + (((IkReal(-0.420000000000000)) * (x412))) + (x418) + + (((IkReal(-1.00000000000000)) * (x415) * (x421)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x411) * (x413) * (x416))) + + (((npx) * (x411) * (x414))) + (((sj4) * (x420))) + + (((cj4) * (npx) * (sj6))) + (((cj4) * (cj6) * (npy)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (sj6) * (x413) * (x421))) + + (((x418) * (x421))) + (((cj4) * (x420))) + + (((IkReal(-1.00000000000000)) * (x413) * (x414))) + + (((IkReal(-1.00000000000000)) * (npx) * (x416))) + + (((IkReal(-1.00000000000000)) * (x415)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + rotationfunction0(solutions); + } + } + } + } + } + } + } + } + } + } + } + return solutions.GetNumSolutions() > 0; + } + inline void rotationfunction0(IkSolutionListBase& solutions) + { + for (int rotationiter = 0; rotationiter < 1; ++rotationiter) + { + IkReal x78 = ((cj6) * (sj4)); + IkReal x79 = ((IkReal(1.00000000000000)) * (sj5)); + IkReal x80 = ((cj4) * (cj5)); + IkReal x81 = ((IkReal(1.00000000000000)) * (sj3)); + IkReal x82 = ((cj3) * (sj5)); + IkReal x83 = ((IkReal(1.00000000000000)) * (sj6)); + IkReal x84 = ((sj4) * (sj6)); + IkReal x85 = ((IkReal(1.00000000000000)) * (cj3)); + IkReal x86 = ((IkReal(-1.00000000000000)) * (cj3)); + IkReal x87 = ((((cj3) * (x80))) + (((IkReal(-1.00000000000000)) * (sj3) * (x79)))); + IkReal x88 = ((((cj5) * (sj3))) + (((cj4) * (x82)))); + IkReal x89 = ((((cj5) * (x84))) + (((IkReal(-1.00000000000000)) * (cj4) * (cj6)))); + IkReal x90 = ((x82) + (((sj3) * (x80)))); + IkReal x91 = ((((cj4) * (sj3) * (sj5))) + (((IkReal(-1.00000000000000)) * (cj5) * (x85)))); + IkReal x92 = ((cj6) * (x87)); + IkReal x93 = ((((IkReal(-1.00000000000000)) * (cj4) * (x83))) + + (((IkReal(-1.00000000000000)) * (cj5) * (x78)))); + IkReal x94 = ((((IkReal(-1.00000000000000)) * (cj3) * (sj4) * (x83))) + (x92)); + IkReal x95 = ((((IkReal(-1.00000000000000)) * (x81) * (x84))) + (((cj6) * (x90)))); + IkReal x96 = ((((IkReal(-1.00000000000000)) * (x83) * (x90))) + + (((IkReal(-1.00000000000000)) * (x78) * (x81)))); + IkReal x97 = ((((x78) * (x86))) + (((IkReal(-1.00000000000000)) * (sj6) * (x87)))); + new_r00 = ((((r00) * (((((x84) * (x86))) + (x92))))) + + (((r01) * (((((IkReal(-1.00000000000000)) * (x83) * (x87))) + + (((IkReal(-1.00000000000000)) * (x78) * (x85))))))) + + (((r02) * (x88)))); + new_r01 = ((((r12) * (x88))) + (((r11) * (x97))) + (((r10) * (x94)))); + new_r02 = ((((r21) * (x97))) + (((r20) * (x94))) + (((r22) * (x88)))); + new_r10 = ((((r00) * (x93))) + (((r01) * (x89))) + + (((IkReal(-1.00000000000000)) * (r02) * (sj4) * (x79)))); + new_r11 = ((((r11) * (x89))) + (((r10) * (x93))) + + (((IkReal(-1.00000000000000)) * (r12) * (sj4) * (x79)))); + new_r12 = ((((r21) * (x89))) + (((IkReal(-1.00000000000000)) * (r22) * (sj4) * (x79))) + + (((r20) * (x93)))); + new_r20 = ((((r01) * (x96))) + (((r00) * (x95))) + (((r02) * (x91)))); + new_r21 = ((((r12) * (x91))) + (((r11) * (x96))) + (((r10) * (x95)))); + new_r22 = ((((r21) * (x96))) + (((r20) * (x95))) + (((r22) * (x91)))); + { + IkReal j1array[2], cj1array[2], sj1array[2]; + bool j1valid[2] = {false}; + _nj1 = 2; + cj1array[0] = new_r22; + if (cj1array[0] >= -1 - IKFAST_SINCOS_THRESH && cj1array[0] <= 1 + IKFAST_SINCOS_THRESH) + { + j1valid[0] = j1valid[1] = true; + j1array[0] = IKacos(cj1array[0]); + sj1array[0] = IKsin(j1array[0]); + cj1array[1] = cj1array[0]; + j1array[1] = -j1array[0]; + sj1array[1] = -sj1array[0]; + } + else if (std::isnan(cj1array[0])) + { + // probably any value will work + j1valid[0] = true; + cj1array[0] = 1; + sj1array[0] = 0; + j1array[0] = 0; + } + for (int ij1 = 0; ij1 < 2; ++ij1) + { + if (!j1valid[ij1]) + { + continue; + } + _ij1[0] = ij1; + _ij1[1] = -1; + for (int iij1 = ij1 + 1; iij1 < 2; ++iij1) + { + if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH && + IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH) + { + j1valid[iij1] = false; + _ij1[1] = iij1; + break; + } + } + j1 = j1array[ij1]; + cj1 = cj1array[ij1]; + sj1 = sj1array[ij1]; + + { + IkReal dummyeval[1]; + IkReal gconst12; + gconst12 = IKsign(sj1); + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst13; + gconst13 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst14; + gconst14 = IKsign(((((cj1) * ((new_r12) * (new_r12)))) + + (((cj1) * ((new_r02) * (new_r02)))))); + dummyeval[0] = + ((((cj1) * ((new_r12) * (new_r12)))) + (((cj1) * ((new_r02) * (new_r02))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[7]; + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.57079632679490)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = new_r22; + evalcond[2] = new_r22; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(new_r21) + IKsqr(new_r20) - 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2(new_r21, new_r20); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[2]; + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (IKcos(j0)))) + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (IKsin(j0)))) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst21; + gconst21 = + IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst20; + gconst20 = IKsign( + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x98 = ((gconst20) * (sj0)); + if (IKabs(((new_r12) * (x98))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * + (x98))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2( + ((new_r12) * (x98)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x98))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x99 = IKsin(j2); + IkReal x100 = IKcos(j2); + IkReal x101 = ((IkReal(1.00000000000000)) * (x99)); + evalcond[0] = + ((((new_r02) * (x99))) + (((new_r12) * (x100)))); + evalcond[1] = ((((new_r10) * (x100))) + (sj0) + + (((new_r00) * (x99)))); + evalcond[2] = ((IkReal(1.00000000000000)) + + (((new_r02) * (x100))) + + (((IkReal(-1.00000000000000)) * + (new_r12) * (x101)))); + evalcond[3] = ((((new_r01) * (x99))) + + (((IkReal(-1.00000000000000)) * (cj0))) + + (((new_r11) * (x100)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * + (new_r10) * (x101))) + + (((new_r00) * (x100)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * + (new_r11) * (x101))) + + (((new_r01) * (x100)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + if (IKabs(((gconst21) * (new_r12))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst21) * + (new_r02))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2( + ((gconst21) * (new_r12)), + ((IkReal(-1.00000000000000)) * (gconst21) * (new_r02))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x102 = IKsin(j2); + IkReal x103 = IKcos(j2); + IkReal x104 = ((IkReal(1.00000000000000)) * (x102)); + evalcond[0] = + ((((new_r02) * (x102))) + (((new_r12) * (x103)))); + evalcond[1] = ((((new_r10) * (x103))) + (sj0) + + (((new_r00) * (x102)))); + evalcond[2] = + ((IkReal(1.00000000000000)) + (((new_r02) * (x103))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (x104)))); + evalcond[3] = ((((new_r01) * (x102))) + + (((IkReal(-1.00000000000000)) * (cj0))) + + (((new_r11) * (x103)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (new_r10) * (x104))) + + (((new_r00) * (x103)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (new_r11) * (x104))) + + (((new_r01) * (x103)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + else + { + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(4.71238898038469)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = new_r22; + evalcond[2] = ((IkReal(-1.00000000000000)) * (new_r22)); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((IkReal(-1.00000000000000)) * (new_r21))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r20))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((IkReal(-1.00000000000000)) * (new_r21))) + + IKsqr(((IkReal(-1.00000000000000)) * (new_r20))) - 1) <= + IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2(((IkReal(-1.00000000000000)) * (new_r21)), + ((IkReal(-1.00000000000000)) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[2]; + evalcond[0] = ((IKcos(j0)) + (new_r20)); + evalcond[1] = ((IKsin(j0)) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst24; + gconst24 = IKsign( + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10))))); + dummyeval[0] = + ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst25; + gconst25 = IKsign(((((IkReal(-1.00000000000000)) * + ((new_r02) * (new_r02)))) + + (((IkReal(-1.00000000000000)) * + ((new_r12) * (new_r12)))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * + ((new_r02) * (new_r02)))) + + (((IkReal(-1.00000000000000)) * + ((new_r12) * (new_r12))))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + if (IKabs(((gconst25) * (new_r12))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (gconst25) * + (new_r02))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((gconst25) * (new_r12)), + ((IkReal(-1.00000000000000)) * + (gconst25) * (new_r02))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x105 = IKsin(j2); + IkReal x106 = IKcos(j2); + IkReal x107 = ((IkReal(1.00000000000000)) * (x105)); + evalcond[0] = + ((((new_r02) * (x105))) + (((new_r12) * (x106)))); + evalcond[1] = ((((new_r10) * (x106))) + (sj0) + + (((new_r00) * (x105)))); + evalcond[2] = ((IkReal(-1.00000000000000)) + + (((new_r02) * (x106))) + + (((IkReal(-1.00000000000000)) * + (new_r12) * (x107)))); + evalcond[3] = + ((((new_r01) * (x105))) + + (((IkReal(-1.00000000000000)) * (cj0))) + + (((new_r11) * (x106)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * + (new_r10) * (x107))) + + (((new_r00) * (x106)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * + (new_r11) * (x107))) + + (((new_r01) * (x106)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos( + 7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x108 = ((gconst24) * (sj0)); + if (IKabs(((new_r12) * (x108))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * + (x108))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2( + ((new_r12) * (x108)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x108))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x109 = IKsin(j2); + IkReal x110 = IKcos(j2); + IkReal x111 = ((IkReal(1.00000000000000)) * (x109)); + evalcond[0] = + ((((new_r02) * (x109))) + (((new_r12) * (x110)))); + evalcond[1] = ((((new_r10) * (x110))) + (sj0) + + (((new_r00) * (x109)))); + evalcond[2] = ((IkReal(-1.00000000000000)) + + (((new_r02) * (x110))) + + (((IkReal(-1.00000000000000)) * (new_r12) * + (x111)))); + evalcond[3] = + ((((new_r11) * (x110))) + (((new_r01) * (x109))) + + (((IkReal(-1.00000000000000)) * (cj0)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * (new_r10) * + (x111))) + + (((new_r00) * (x110)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * (new_r11) * + (x111))) + + (((new_r01) * (x110)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + else + { + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[5] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[6] = ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000) + { + { + IkReal j2array[2], cj2array[2], sj2array[2]; + bool j2valid[2] = {false}; + _nj2 = 2; + if (IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x112 = IKatan2(new_r12, new_r02); + j2array[0] = ((IkReal(-1.00000000000000)) * (x112)); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + j2array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x112)))); + sj2array[1] = IKsin(j2array[1]); + cj2array[1] = IKcos(j2array[1]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + if (j2array[1] > IKPI) + { + j2array[1] -= IK2PI; + } + else if (j2array[1] < -IKPI) + { + j2array[1] += IK2PI; + } + j2valid[1] = true; + 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 evalcond[1]; + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (new_r12) * (IKsin(j2)))) + + (((new_r02) * (IKcos(j2))))); + if (IKabs(evalcond[0]) > 0.000001) + { + continue; + } + } + + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs( + IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x113 = IKsin(j0); + IkReal x114 = ((IkReal(1.00000000000000)) * (sj2)); + IkReal x115 = ((IkReal(1.00000000000000)) * (IKcos(j0))); + evalcond[0] = ((((new_r00) * (sj2))) + (x113) + + (((cj2) * (new_r10)))); + evalcond[1] = ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x115))) + + (((cj2) * (new_r11)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r10) * (x114))) + + (((IkReal(-1.00000000000000)) * (x115))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (new_r11) * (x114))) + + (((cj2) * (new_r01))) + + (((IkReal(-1.00000000000000)) * (x113)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + IkReal x116 = ((IkReal(1.00000000000000)) + (new_r22)); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = x116; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = new_r20; + evalcond[5] = new_r21; + evalcond[6] = x116; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000) + { + { + IkReal j2array[2], cj2array[2], sj2array[2]; + bool j2valid[2] = {false}; + _nj2 = 2; + if (IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && + IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH) + continue; + IkReal x117 = IKatan2(new_r12, new_r02); + j2array[0] = ((IkReal(-1.00000000000000)) * (x117)); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + j2array[1] = ((IkReal(3.14159265358979)) + + (((IkReal(-1.00000000000000)) * (x117)))); + sj2array[1] = IKsin(j2array[1]); + cj2array[1] = IKcos(j2array[1]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + if (j2array[1] > IKPI) + { + j2array[1] -= IK2PI; + } + else if (j2array[1] < -IKPI) + { + j2array[1] += IK2PI; + } + j2valid[1] = true; + 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 evalcond[1]; + evalcond[0] = ((((IkReal(-1.00000000000000)) * (new_r12) * + (IKsin(j2)))) + + (((new_r02) * (IKcos(j2))))); + if (IKabs(evalcond[0]) > 0.000001) + { + continue; + } + } + + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x118 = IKcos(j0); + IkReal x119 = IKsin(j0); + IkReal x120 = ((IkReal(1.00000000000000)) * (sj2)); + evalcond[0] = ((((new_r00) * (sj2))) + (x119) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x118)))); + evalcond[2] = ((((IkReal(-1.00000000000000)) * (new_r10) * + (x120))) + + (x118) + (((cj2) * (new_r00)))); + evalcond[3] = ((x119) + (((cj2) * (new_r01))) + + (((IkReal(-1.00000000000000)) * (new_r11) * + (x120)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x121 = ((gconst14) * (new_r22) * (sj1)); + if (IKabs(((new_r12) * (x121))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x121))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((new_r12) * (x121)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x121))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x122 = IKcos(j2); + IkReal x123 = IKsin(j2); + IkReal x124 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x125 = ((sj1) * (x122)); + IkReal x126 = ((new_r12) * (x123)); + IkReal x127 = ((new_r02) * (x122)); + IkReal x128 = ((IkReal(1.00000000000000)) * (sj1) * (x123)); + evalcond[0] = ((((new_r02) * (x123))) + (((new_r12) * (x122)))); + evalcond[1] = + ((sj1) + (x127) + (((IkReal(-1.00000000000000)) * (x126)))); + evalcond[2] = ((((cj1) * (x127))) + + (((IkReal(-1.00000000000000)) * (x124) * (x126))) + + (((new_r22) * (sj1)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (new_r10) * (x128))) + + (((new_r00) * (x125))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x124)))); + evalcond[4] = ((((new_r01) * (x125))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x124))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x128)))); + evalcond[5] = ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (sj1) * (x126))) + + (((new_r02) * (x125))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x124)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst15; + gconst15 = IKsign(sj1); + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[11]; + IkReal x129 = + ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + IkReal x130 = + ((((cj2) * (new_r02))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj2)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x129; + evalcond[5] = x129; + evalcond[6] = x130; + evalcond[7] = x130; + evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[9] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[10] = + ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * + (new_r00) * (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x131 = IKsin(j0); + IkReal x132 = ((IkReal(1.00000000000000)) * (sj2)); + IkReal x133 = + ((IkReal(1.00000000000000)) * (IKcos(j0))); + evalcond[0] = ((((new_r00) * (sj2))) + (x131) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x133))) + + (((cj2) * (new_r11)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r10) * + (x132))) + + (((IkReal(-1.00000000000000)) * (x133))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (new_r11) * + (x132))) + + (((cj2) * (new_r01))) + + (((IkReal(-1.00000000000000)) * (x131)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > + vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + IkReal x134 = ((IkReal(1.00000000000000)) + (new_r22)); + IkReal x135 = ((new_r12) * (sj2)); + IkReal x136 = ((cj2) * (new_r02)); + IkReal x137 = + ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = x134; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x137; + evalcond[5] = x137; + evalcond[6] = + ((x136) + (((IkReal(-1.00000000000000)) * (x135)))); + evalcond[7] = + ((x135) + (((IkReal(-1.00000000000000)) * (x136)))); + evalcond[8] = new_r20; + evalcond[9] = new_r21; + evalcond[10] = x134; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * + (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * + (new_r00) * (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x138 = IKcos(j0); + IkReal x139 = IKsin(j0); + IkReal x140 = + ((IkReal(1.00000000000000)) * (sj2)); + evalcond[0] = ((((new_r00) * (sj2))) + (x139) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + + (((cj2) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x138)))); + evalcond[2] = + ((x138) + (((IkReal(-1.00000000000000)) * + (new_r10) * (x140))) + + (((cj2) * (new_r00)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * + (new_r11) * (x140))) + + (x139) + (((cj2) * (new_r01)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > + vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((new_r20) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((new_r20) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((new_r20) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x141 = IKsin(j0); + IkReal x142 = IKcos(j0); + IkReal x143 = ((cj2) * (new_r01)); + IkReal x144 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x145 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x146 = ((new_r10) * (sj2)); + IkReal x147 = ((new_r11) * (sj2)); + IkReal x148 = ((cj2) * (new_r00)); + IkReal x149 = ((IkReal(1.00000000000000)) * (x142)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x142) * (x145))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x141) * (x145))) + + (new_r21)); + evalcond[2] = ((((new_r00) * (sj2))) + (x141) + + (((cj2) * (new_r10)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x149))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x142) * (x144))) + + (x148) + (((IkReal(-1.00000000000000)) * (x146)))); + evalcond[5] = + ((x143) + (((IkReal(-1.00000000000000)) * (x147))) + + (((IkReal(-1.00000000000000)) * (x141) * (x144)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x149))) + + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x144) * (x146))) + + (((cj1) * (x148)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x144) * (x147))) + + (((IkReal(-1.00000000000000)) * (x141))) + + (((new_r21) * (sj1))) + (((cj1) * (x143)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs( + ((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs( + IKsqr(((new_r21) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) + + IKsqr(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x150 = IKsin(j0); + IkReal x151 = IKcos(j0); + IkReal x152 = ((cj2) * (new_r01)); + IkReal x153 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x154 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x155 = ((new_r10) * (sj2)); + IkReal x156 = ((new_r11) * (sj2)); + IkReal x157 = ((cj2) * (new_r00)); + IkReal x158 = ((IkReal(1.00000000000000)) * (x151)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x151) * (x154))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x150) * (x154))) + + (new_r21)); + evalcond[2] = ((((new_r00) * (sj2))) + (x150) + + (((cj2) * (new_r10)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x158))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x151) * (x153))) + + (x157) + (((IkReal(-1.00000000000000)) * (x155)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x150) * (x153))) + + (x152) + (((IkReal(-1.00000000000000)) * (x156)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x158))) + + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x153) * (x155))) + + (((cj1) * (x157)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x153) * (x156))) + + (((IkReal(-1.00000000000000)) * (x150))) + + (((new_r21) * (sj1))) + (((cj1) * (x152)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((gconst15) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst15) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j0array[0] = + IKatan2(((gconst15) * (new_r21)), ((gconst15) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x159 = IKsin(j0); + IkReal x160 = IKcos(j0); + IkReal x161 = ((cj2) * (new_r01)); + IkReal x162 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x163 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x164 = ((new_r10) * (sj2)); + IkReal x165 = ((new_r11) * (sj2)); + IkReal x166 = ((cj2) * (new_r00)); + IkReal x167 = ((IkReal(1.00000000000000)) * (x160)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x160) * (x163))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x159) * (x163))) + + (new_r21)); + evalcond[2] = + ((((new_r00) * (sj2))) + (x159) + (((cj2) * (new_r10)))); + evalcond[3] = ((((IkReal(-1.00000000000000)) * (x167))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x160) * (x162))) + + (x166) + (((IkReal(-1.00000000000000)) * (x164)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x159) * (x162))) + + (x161) + (((IkReal(-1.00000000000000)) * (x165)))); + evalcond[6] = + ((((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x167))) + + (((IkReal(-1.00000000000000)) * (x162) * (x164))) + + (((cj1) * (x166)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x162) * (x165))) + + (((IkReal(-1.00000000000000)) * (x159))) + + (((cj1) * (x161))) + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x168 = ((gconst13) * (sj1)); + if (IKabs(((new_r12) * (x168))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x168))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((new_r12) * (x168)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x168))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[6]; + IkReal x169 = IKcos(j2); + IkReal x170 = IKsin(j2); + IkReal x171 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x172 = ((sj1) * (x169)); + IkReal x173 = ((new_r12) * (x170)); + IkReal x174 = ((new_r02) * (x169)); + IkReal x175 = ((IkReal(1.00000000000000)) * (sj1) * (x170)); + evalcond[0] = ((((new_r02) * (x170))) + (((new_r12) * (x169)))); + evalcond[1] = ((sj1) + (((IkReal(-1.00000000000000)) * (x173))) + (x174)); + evalcond[2] = ((((new_r22) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x171) * (x173))) + + (((cj1) * (x174)))); + evalcond[3] = ((((new_r00) * (x172))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x171))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x175)))); + evalcond[4] = ((((new_r01) * (x172))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x171))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x175)))); + evalcond[5] = ((IkReal(1.00000000000000)) + (((new_r02) * (x172))) + + (((IkReal(-1.00000000000000)) * (sj1) * (x173))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x171)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst15; + gconst15 = IKsign(sj1); + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + dummyeval[0] = sj1; + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal evalcond[11]; + IkReal x176 = ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + IkReal x177 = + ((((cj2) * (new_r02))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj2)))); + evalcond[0] = ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(3.14159265358979)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = ((IkReal(-1.00000000000000)) + (new_r22)); + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x176; + evalcond[5] = x176; + evalcond[6] = x177; + evalcond[7] = x177; + evalcond[8] = ((IkReal(-1.00000000000000)) * (new_r20)); + evalcond[9] = ((IkReal(-1.00000000000000)) * (new_r21)); + evalcond[10] = ((IkReal(1.00000000000000)) + + (((IkReal(-1.00000000000000)) * (new_r22)))); + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x178 = IKsin(j0); + IkReal x179 = ((IkReal(1.00000000000000)) * (sj2)); + IkReal x180 = + ((IkReal(1.00000000000000)) * (IKcos(j0))); + evalcond[0] = ((((new_r00) * (sj2))) + (x178) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x180))) + + (((new_r01) * (sj2))) + (((cj2) * (new_r11)))); + evalcond[2] = + ((((IkReal(-1.00000000000000)) * (new_r10) * + (x179))) + + (((IkReal(-1.00000000000000)) * (x180))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((((IkReal(-1.00000000000000)) * (x178))) + + (((IkReal(-1.00000000000000)) * (new_r11) * + (x179))) + + (((cj2) * (new_r01)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + IkReal x181 = ((IkReal(1.00000000000000)) + (new_r22)); + IkReal x182 = ((new_r12) * (sj2)); + IkReal x183 = ((cj2) * (new_r02)); + IkReal x184 = ((((new_r02) * (sj2))) + (((cj2) * (new_r12)))); + evalcond[0] = + ((IkReal(-3.14159265358979)) + + (IKfmod(((IkReal(1.11022302462516e-16)) + (j1)), + IkReal(6.28318530717959)))); + evalcond[1] = x181; + evalcond[2] = new_r20; + evalcond[3] = new_r21; + evalcond[4] = x184; + evalcond[5] = x184; + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x182))) + (x183)); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x183))) + (x182)); + evalcond[8] = new_r20; + evalcond[9] = new_r21; + evalcond[10] = x181; + if (IKabs(evalcond[0]) < 0.0000010000000000 && + IKabs(evalcond[1]) < 0.0000010000000000 && + IKabs(evalcond[2]) < 0.0000010000000000 && + IKabs(evalcond[3]) < 0.0000010000000000 && + IKabs(evalcond[4]) < 0.0000010000000000 && + IKabs(evalcond[5]) < 0.0000010000000000 && + IKabs(evalcond[6]) < 0.0000010000000000 && + IKabs(evalcond[7]) < 0.0000010000000000 && + IKabs(evalcond[8]) < 0.0000010000000000 && + IKabs(evalcond[9]) < 0.0000010000000000 && + IKabs(evalcond[10]) < 0.0000010000000000) + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs( + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * + (new_r00) * (sj2))))) + + IKsqr(((((new_r01) * (sj2))) + + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[4]; + IkReal x185 = IKcos(j0); + IkReal x186 = IKsin(j0); + IkReal x187 = ((IkReal(1.00000000000000)) * (sj2)); + evalcond[0] = ((((new_r00) * (sj2))) + (x186) + + (((cj2) * (new_r10)))); + evalcond[1] = + ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x185))) + + (((cj2) * (new_r11)))); + evalcond[2] = + ((x185) + (((IkReal(-1.00000000000000)) * + (new_r10) * (x187))) + + (((cj2) * (new_r00)))); + evalcond[3] = + ((x186) + (((IkReal(-1.00000000000000)) * + (new_r11) * (x187))) + + (((cj2) * (new_r01)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos( + 7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + else + { + if (1) + { + continue; + } + else + { + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) < IKFAST_ATAN2_MAGTHRESH && + IKabs( + ((new_r20) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((((IkReal(-1.00000000000000)) * (cj2) * + (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * + (sj2))))) + + IKsqr(((new_r20) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = IKatan2( + ((((IkReal(-1.00000000000000)) * (cj2) * (new_r10))) + + (((IkReal(-1.00000000000000)) * (new_r00) * (sj2)))), + ((new_r20) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x188 = IKsin(j0); + IkReal x189 = IKcos(j0); + IkReal x190 = ((cj2) * (new_r01)); + IkReal x191 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x192 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x193 = ((new_r10) * (sj2)); + IkReal x194 = ((new_r11) * (sj2)); + IkReal x195 = ((cj2) * (new_r00)); + IkReal x196 = ((IkReal(1.00000000000000)) * (x189)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x189) * (x192))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x188) * (x192))) + + (new_r21)); + evalcond[2] = ((((new_r00) * (sj2))) + (x188) + + (((cj2) * (new_r10)))); + evalcond[3] = ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x196))) + + (((cj2) * (new_r11)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (x189) * (x191))) + + (((IkReal(-1.00000000000000)) * (x193))) + (x195)); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x188) * (x191))) + + (((IkReal(-1.00000000000000)) * (x194))) + (x190)); + evalcond[6] = + ((((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x191) * (x193))) + + (((IkReal(-1.00000000000000)) * (x196))) + + (((cj1) * (x195)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x191) * (x194))) + + (((IkReal(-1.00000000000000)) * (x188))) + + (((cj1) * (x190))) + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) < + IKFAST_ATAN2_MAGTHRESH && + IKabs(IKsqr(((new_r21) * + (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30)))) + + IKsqr(((((new_r01) * (sj2))) + (((cj2) * (new_r11))))) - + 1) <= IKFAST_SINCOS_THRESH) + continue; + j0array[0] = + IKatan2(((new_r21) * (((IKabs(sj1) != 0) ? ((IkReal)1 / (sj1)) + : (IkReal)1.0e30))), + ((((new_r01) * (sj2))) + (((cj2) * (new_r11))))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < + IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x197 = IKsin(j0); + IkReal x198 = IKcos(j0); + IkReal x199 = ((cj2) * (new_r01)); + IkReal x200 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x201 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x202 = ((new_r10) * (sj2)); + IkReal x203 = ((new_r11) * (sj2)); + IkReal x204 = ((cj2) * (new_r00)); + IkReal x205 = ((IkReal(1.00000000000000)) * (x198)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x198) * (x201))) + + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x197) * (x201))) + + (new_r21)); + evalcond[2] = + ((((new_r00) * (sj2))) + (x197) + (((cj2) * (new_r10)))); + evalcond[3] = ((((new_r01) * (sj2))) + + (((IkReal(-1.00000000000000)) * (x205))) + + (((cj2) * (new_r11)))); + evalcond[4] = + ((x204) + + (((IkReal(-1.00000000000000)) * (x198) * (x200))) + + (((IkReal(-1.00000000000000)) * (x202)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (x197) * (x200))) + + (x199) + (((IkReal(-1.00000000000000)) * (x203)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (x200) * (x202))) + + (((cj1) * (x204))) + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x205)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (x200) * (x203))) + + (((IkReal(-1.00000000000000)) * (x197))) + + (((cj1) * (x199))) + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((gconst15) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst15) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j0array[0] = + IKatan2(((gconst15) * (new_r21)), ((gconst15) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < + IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[8]; + IkReal x206 = IKsin(j0); + IkReal x207 = IKcos(j0); + IkReal x208 = ((cj2) * (new_r01)); + IkReal x209 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x210 = ((IkReal(1.00000000000000)) * (sj1)); + IkReal x211 = ((new_r10) * (sj2)); + IkReal x212 = ((new_r11) * (sj2)); + IkReal x213 = ((cj2) * (new_r00)); + IkReal x214 = ((IkReal(1.00000000000000)) * (x207)); + evalcond[0] = + ((((IkReal(-1.00000000000000)) * (x207) * (x210))) + (new_r20)); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x206) * (x210))) + (new_r21)); + evalcond[2] = + ((((new_r00) * (sj2))) + (x206) + (((cj2) * (new_r10)))); + evalcond[3] = ((((new_r01) * (sj2))) + (((cj2) * (new_r11))) + + (((IkReal(-1.00000000000000)) * (x214)))); + evalcond[4] = ((((IkReal(-1.00000000000000)) * (x207) * (x209))) + + (x213) + (((IkReal(-1.00000000000000)) * (x211)))); + evalcond[5] = ((((IkReal(-1.00000000000000)) * (x206) * (x209))) + + (x208) + (((IkReal(-1.00000000000000)) * (x212)))); + evalcond[6] = ((((new_r20) * (sj1))) + (((cj1) * (x213))) + + (((IkReal(-1.00000000000000)) * (x214))) + + (((IkReal(-1.00000000000000)) * (x209) * (x211)))); + evalcond[7] = ((((cj1) * (x208))) + (((new_r21) * (sj1))) + + (((IkReal(-1.00000000000000)) * (x206))) + + (((IkReal(-1.00000000000000)) * (x209) * (x212)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + } + } + else + { + { + IkReal j0array[1], cj0array[1], sj0array[1]; + bool j0valid[1] = {false}; + _nj0 = 1; + if (IKabs(((gconst12) * (new_r21))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((gconst12) * (new_r20))) < IKFAST_ATAN2_MAGTHRESH) + continue; + j0array[0] = IKatan2(((gconst12) * (new_r21)), ((gconst12) * (new_r20))); + sj0array[0] = IKsin(j0array[0]); + cj0array[0] = IKcos(j0array[0]); + if (j0array[0] > IKPI) + { + j0array[0] -= IK2PI; + } + else if (j0array[0] < -IKPI) + { + j0array[0] += IK2PI; + } + j0valid[0] = true; + for (int ij0 = 0; ij0 < 1; ++ij0) + { + if (!j0valid[ij0]) + { + continue; + } + _ij0[0] = ij0; + _ij0[1] = -1; + for (int iij0 = ij0 + 1; iij0 < 1; ++iij0) + { + if (j0valid[iij0] && + IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH && + IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH) + { + j0valid[iij0] = false; + _ij0[1] = iij0; + break; + } + } + j0 = j0array[ij0]; + cj0 = cj0array[ij0]; + sj0 = sj0array[ij0]; + { + IkReal evalcond[2]; + IkReal x215 = ((IkReal(1.00000000000000)) * (sj1)); + evalcond[0] = + ((new_r20) + (((IkReal(-1.00000000000000)) * (x215) * (IKcos(j0))))); + evalcond[1] = + ((((IkReal(-1.00000000000000)) * (x215) * (IKsin(j0)))) + (new_r21)); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001) + { + continue; + } + } + + { + IkReal dummyeval[1]; + IkReal gconst17; + gconst17 = IKsign((((new_r12) * (new_r12)) + ((new_r02) * (new_r02)))); + dummyeval[0] = (((new_r12) * (new_r12)) + ((new_r02) * (new_r02))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + { + IkReal dummyeval[1]; + IkReal gconst16; + gconst16 = IKsign(((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10))))); + dummyeval[0] = ((((IkReal(-1.00000000000000)) * (new_r00) * (new_r12))) + + (((new_r02) * (new_r10)))); + if (IKabs(dummyeval[0]) < 0.0000010000000000) + { + continue; + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x216 = ((gconst16) * (sj0)); + if (IKabs(((new_r12) * (x216))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x216))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = + IKatan2(((new_r12) * (x216)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x216))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[12]; + IkReal x217 = IKsin(j2); + IkReal x218 = IKcos(j2); + IkReal x219 = ((IkReal(1.00000000000000)) * (cj0)); + IkReal x220 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x221 = ((IkReal(1.00000000000000)) * (x217)); + IkReal x222 = ((new_r01) * (x218)); + IkReal x223 = ((new_r00) * (x218)); + IkReal x224 = ((new_r02) * (x218)); + evalcond[0] = ((((new_r02) * (x217))) + (((new_r12) * (x218)))); + evalcond[1] = + ((sj0) + (((new_r00) * (x217))) + (((new_r10) * (x218)))); + evalcond[2] = + ((sj1) + (x224) + + (((IkReal(-1.00000000000000)) * (new_r12) * (x221)))); + evalcond[3] = ((((new_r01) * (x217))) + + (((IkReal(-1.00000000000000)) * (x219))) + + (((new_r11) * (x218)))); + evalcond[4] = + ((((IkReal(-1.00000000000000)) * (cj1) * (x219))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x221))) + (x223)); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (new_r11) * (x221))) + + (((IkReal(-1.00000000000000)) * (sj0) * (x220))) + (x222)); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (new_r12) * (x217) * (x220))) + + (((cj1) * (x224))) + (((new_r22) * (sj1)))); + evalcond[7] = + ((((IkReal(-1.00000000000000)) * (new_r10) * (sj1) * (x221))) + + (((sj1) * (x223))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x220)))); + evalcond[8] = + ((((sj1) * (x222))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (sj1) * (x221))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x220)))); + evalcond[9] = + ((IkReal(1.00000000000000)) + (((sj1) * (x224))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x220))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj1) * (x221)))); + evalcond[10] = + ((((new_r20) * (sj1))) + (((cj1) * (x223))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x217) * (x220))) + + (((IkReal(-1.00000000000000)) * (x219)))); + evalcond[11] = + ((((IkReal(-1.00000000000000)) * (sj0))) + (((cj1) * (x222))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x217) * (x220))) + + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || + IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || + IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || + IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || + IKabs(evalcond[7]) > 0.000001 || + IKabs(evalcond[8]) > 0.000001 || + IKabs(evalcond[9]) > 0.000001 || + IKabs(evalcond[10]) > 0.000001 || + IKabs(evalcond[11]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + else + { + { + IkReal j2array[1], cj2array[1], sj2array[1]; + bool j2valid[1] = {false}; + _nj2 = 1; + IkReal x225 = ((gconst17) * (sj1)); + if (IKabs(((new_r12) * (x225))) < IKFAST_ATAN2_MAGTHRESH && + IKabs(((IkReal(-1.00000000000000)) * (new_r02) * (x225))) < + IKFAST_ATAN2_MAGTHRESH) + continue; + j2array[0] = IKatan2(((new_r12) * (x225)), + ((IkReal(-1.00000000000000)) * (new_r02) * (x225))); + sj2array[0] = IKsin(j2array[0]); + cj2array[0] = IKcos(j2array[0]); + if (j2array[0] > IKPI) + { + j2array[0] -= IK2PI; + } + else if (j2array[0] < -IKPI) + { + j2array[0] += IK2PI; + } + j2valid[0] = true; + for (int ij2 = 0; ij2 < 1; ++ij2) + { + if (!j2valid[ij2]) + { + continue; + } + _ij2[0] = ij2; + _ij2[1] = -1; + for (int iij2 = ij2 + 1; iij2 < 1; ++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 evalcond[12]; + IkReal x226 = IKsin(j2); + IkReal x227 = IKcos(j2); + IkReal x228 = ((IkReal(1.00000000000000)) * (cj0)); + IkReal x229 = ((IkReal(1.00000000000000)) * (cj1)); + IkReal x230 = ((IkReal(1.00000000000000)) * (x226)); + IkReal x231 = ((new_r01) * (x227)); + IkReal x232 = ((new_r00) * (x227)); + IkReal x233 = ((new_r02) * (x227)); + evalcond[0] = ((((new_r02) * (x226))) + (((new_r12) * (x227)))); + evalcond[1] = ((((new_r00) * (x226))) + (sj0) + (((new_r10) * (x227)))); + evalcond[2] = + ((sj1) + (((IkReal(-1.00000000000000)) * (new_r12) * (x230))) + + (x233)); + evalcond[3] = ((((new_r01) * (x226))) + (((new_r11) * (x227))) + + (((IkReal(-1.00000000000000)) * (x228)))); + evalcond[4] = + ((x232) + (((IkReal(-1.00000000000000)) * (cj1) * (x228))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x230)))); + evalcond[5] = + ((((IkReal(-1.00000000000000)) * (sj0) * (x229))) + (x231) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x230)))); + evalcond[6] = + ((((IkReal(-1.00000000000000)) * (new_r12) * (x226) * (x229))) + + (((cj1) * (x233))) + (((new_r22) * (sj1)))); + evalcond[7] = + ((((sj1) * (x232))) + + (((IkReal(-1.00000000000000)) * (new_r20) * (x229))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (sj1) * (x230)))); + evalcond[8] = + ((((sj1) * (x231))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (sj1) * (x230))) + + (((IkReal(-1.00000000000000)) * (new_r21) * (x229)))); + evalcond[9] = + ((IkReal(1.00000000000000)) + (((sj1) * (x233))) + + (((IkReal(-1.00000000000000)) * (new_r22) * (x229))) + + (((IkReal(-1.00000000000000)) * (new_r12) * (sj1) * (x230)))); + evalcond[10] = + ((((cj1) * (x232))) + (((new_r20) * (sj1))) + + (((IkReal(-1.00000000000000)) * (new_r10) * (x226) * (x229))) + + (((IkReal(-1.00000000000000)) * (x228)))); + evalcond[11] = + ((((cj1) * (x231))) + (((IkReal(-1.00000000000000)) * (sj0))) + + (((IkReal(-1.00000000000000)) * (new_r11) * (x226) * (x229))) + + (((new_r21) * (sj1)))); + if (IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || + IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || + IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || + IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || + IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || + IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001) + { + continue; + } + } + + { + std::vector > vinfos(7); + 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; + vinfos[6].jointtype = 1; + vinfos[6].foffset = j6; + vinfos[6].indices[0] = _ij6[0]; + vinfos[6].indices[1] = _ij6[1]; + vinfos[6].maxsolutions = _nj6; + std::vector vfree(0); + solutions.AddSolution(vinfos, vfree); + } + } + } + } + } + } + } + } + } + } + } + } + } +}; -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x265=((gconst8)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x265))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x265))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x265)), ((IkReal(-49.0000000000000))*(npx)*(x265))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x266=IKcos(j6); -IkReal x267=IKsin(j6); -evalcond[0]=((((npx)*(x267)))+(((npy)*(x266)))); -evalcond[1]=((((npx)*(x266)))+(((IkReal(-1.00000000000000))*(npy)*(x267)))+(((IkReal(0.490000000000000))*(sj3)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) +/// 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) { -continue; -} -} - -rotationfunction0(solutions); -} + IKSolver solver; + return solver.ComputeIk(eetrans, eerot, pfree, solutions); } -} - -} - -} else -{ -IkReal x268=((IkReal(1.00000000000000))*(pp)); -IkReal x269=((IkReal(0.490000000000000))*(cj3)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j4)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(0.416500000000000))+(((IkReal(-1.00000000000000))*(x268)))+(((IkReal(0.411600000000000))*(cj3)))); -evalcond[2]=((IkReal(0.420000000000000))+(((IkReal(-1.00000000000000))*(npz)))+(x269)); -evalcond[3]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(npz)))+(((IkReal(-1.00000000000000))*(x268)))); -evalcond[4]=((IkReal(-0.420000000000000))+(npz)+(((IkReal(-1.00000000000000))*(x269)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst9; -gconst9=IKsign(((((IkReal(-100.000000000000))*((npy)*(npy))))+(((IkReal(-100.000000000000))*((npx)*(npx)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((npy)*(npy))))+(((IkReal(-1.00000000000000))*((npx)*(npx))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x270=((gconst9)*(sj3)); -if( IKabs(((IkReal(49.0000000000000))*(npy)*(x270))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-49.0000000000000))*(npx)*(x270))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((IkReal(49.0000000000000))*(npy)*(x270)), ((IkReal(-49.0000000000000))*(npx)*(x270))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x271=IKcos(j6); -IkReal x272=IKsin(j6); -evalcond[0]=((((npy)*(x271)))+(((npx)*(x272)))); -evalcond[1]=((((IkReal(-0.490000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(npy)*(x272)))+(((npx)*(x271)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) +IKFAST_API const char* GetKinematicsHash() { -continue; -} -} - -rotationfunction0(solutions); -} -} - + return ""; } -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x273=(sj4)*(sj4); -IkReal x274=((cj4)*(sj4)); -IkReal x275=((IkReal(49.0000000000000))*(npx)*(sj3)); -IkReal x276=((IkReal(49.0000000000000))*(npy)*(sj3)); -if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x275)))+(((x274)*(x276))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x276)))+(((IkReal(-1.00000000000000))*(x274)*(x275))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x275)))+(((x274)*(x276)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(x273)*(x276)))+(((IkReal(-1.00000000000000))*(x274)*(x275))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x277=IKcos(j6); -IkReal x278=IKsin(j6); -IkReal x279=((IkReal(1.00000000000000))*(sj4)); -IkReal x280=((IkReal(0.490000000000000))*(sj3)); -IkReal x281=((npy)*(x277)); -IkReal x282=((npx)*(x277)); -IkReal x283=((npx)*(x278)); -IkReal x284=((npy)*(x278)); -evalcond[0]=((x283)+(x281)+(((sj4)*(x280)))); -evalcond[1]=((x282)+(((IkReal(-1.00000000000000))*(x284)))+(((cj4)*(x280)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(x279)*(x282)))+(((cj4)*(x283)))+(((cj4)*(x281)))+(((sj4)*(x284)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(x282)))+(((IkReal(-1.00000000000000))*(x280)))+(((IkReal(-1.00000000000000))*(x279)*(x281)))+(((IkReal(-1.00000000000000))*(x279)*(x283)))+(((cj4)*(x284)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x285=((IkReal(49.0000000000000))*(cj4)*(sj3)); -IkReal x286=((IkReal(49.0000000000000))*(sj3)*(sj4)); -if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(npx)*(x286)))+(((npy)*(x285))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(npy)*(x286)))+(((IkReal(-1.00000000000000))*(npx)*(x285))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(npx)*(x286)))+(((npy)*(x285)))))), ((gconst6)*(((((IkReal(-1.00000000000000))*(npy)*(x286)))+(((IkReal(-1.00000000000000))*(npx)*(x285))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[4]; -IkReal x287=IKcos(j6); -IkReal x288=IKsin(j6); -IkReal x289=((IkReal(1.00000000000000))*(sj4)); -IkReal x290=((IkReal(0.490000000000000))*(sj3)); -IkReal x291=((npy)*(x287)); -IkReal x292=((npx)*(x287)); -IkReal x293=((npx)*(x288)); -IkReal x294=((npy)*(x288)); -evalcond[0]=((((sj4)*(x290)))+(x291)+(x293)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x294)))+(((cj4)*(x290)))+(x292)); -evalcond[2]=((((IkReal(-1.00000000000000))*(x289)*(x292)))+(((cj4)*(x291)))+(((cj4)*(x293)))+(((sj4)*(x294)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(cj4)*(x292)))+(((IkReal(-1.00000000000000))*(x289)*(x293)))+(((IkReal(-1.00000000000000))*(x289)*(x291)))+(((IkReal(-1.00000000000000))*(x290)))+(((cj4)*(x294)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x295=((IkReal(2500.00000000000))*(pp)); -IkReal x296=((IkReal(2100.00000000000))*(cj5)*(npz)); -IkReal x297=((IkReal(1029.00000000000))*(sj3)*(sj4)*(sj5)); -if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x295)))+(((IkReal(-1.00000000000000))*(npy)*(x296)))+(((IkReal(-1.00000000000000))*(npx)*(x297)))+(((IkReal(159.250000000000))*(npy))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x297)))+(((npx)*(x295)))+(((npx)*(x296)))+(((IkReal(-159.250000000000))*(npx))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x295)))+(((IkReal(-1.00000000000000))*(npy)*(x296)))+(((IkReal(-1.00000000000000))*(npx)*(x297)))+(((IkReal(159.250000000000))*(npy)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(npy)*(x297)))+(((npx)*(x295)))+(((npx)*(x296)))+(((IkReal(-159.250000000000))*(npx))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[6]; -IkReal x298=IKcos(j6); -IkReal x299=IKsin(j6); -IkReal x300=((IkReal(1.00000000000000))*(sj4)); -IkReal x301=((cj5)*(npz)); -IkReal x302=((cj4)*(cj5)); -IkReal x303=((IkReal(0.490000000000000))*(sj3)); -IkReal x304=((IkReal(0.490000000000000))*(cj3)); -IkReal x305=((IkReal(0.840000000000000))*(sj5)); -IkReal x306=((npz)*(sj5)); -IkReal x307=((npy)*(x298)); -IkReal x308=((npx)*(x298)); -IkReal x309=((npy)*(x299)); -IkReal x310=((npx)*(x299)); -evalcond[0]=((x307)+(x310)+(((sj4)*(x303)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(x305)*(x309)))+(((x305)*(x308)))+(((IkReal(-0.840000000000000))*(x301)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((sj5)*(x308)))+(((IkReal(-1.00000000000000))*(x301)))+(((IkReal(-1.00000000000000))*(sj5)*(x309)))+(((IkReal(-1.00000000000000))*(x304)))); -evalcond[3]=((((IkReal(-0.420000000000000))*(sj5)))+(((IkReal(-1.00000000000000))*(x302)*(x303)))+(((IkReal(-1.00000000000000))*(x309)))+(x308)+(((IkReal(-1.00000000000000))*(sj5)*(x304)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(cj5)*(x300)*(x309)))+(((cj5)*(sj4)*(x308)))+(((cj4)*(x310)))+(((sj4)*(x306)))+(((cj4)*(x307)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x302)*(x309)))+(((IkReal(-1.00000000000000))*(x300)*(x310)))+(((x302)*(x308)))+(((IkReal(-1.00000000000000))*(x300)*(x307)))+(((cj4)*(x306)))+(((IkReal(-1.00000000000000))*(x303)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j6array[1], cj6array[1], sj6array[1]; -bool j6valid[1]={false}; -_nj6 = 1; -IkReal x311=((IkReal(49.0000000000000))*(npx)); -IkReal x312=((IkReal(49.0000000000000))*(npy)); -IkReal x313=((sj3)*(sj4)*(sj5)); -IkReal x314=((IkReal(100.000000000000))*(cj5)*(npz)); -if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(npy)*(x314)))+(((IkReal(-1.00000000000000))*(x311)*(x313)))+(((IkReal(-1.00000000000000))*(cj3)*(x312)))+(((IkReal(-42.0000000000000))*(npy))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(x312)*(x313)))+(((npx)*(x314)))+(((cj3)*(x311)))+(((IkReal(42.0000000000000))*(npx))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j6array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(npy)*(x314)))+(((IkReal(-1.00000000000000))*(x311)*(x313)))+(((IkReal(-1.00000000000000))*(cj3)*(x312)))+(((IkReal(-42.0000000000000))*(npy)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(x312)*(x313)))+(((npx)*(x314)))+(((cj3)*(x311)))+(((IkReal(42.0000000000000))*(npx))))))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -for(int ij6 = 0; ij6 < 1; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 1; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[6]; -IkReal x315=IKcos(j6); -IkReal x316=IKsin(j6); -IkReal x317=((IkReal(1.00000000000000))*(sj4)); -IkReal x318=((cj5)*(npz)); -IkReal x319=((cj4)*(cj5)); -IkReal x320=((IkReal(0.490000000000000))*(sj3)); -IkReal x321=((IkReal(0.490000000000000))*(cj3)); -IkReal x322=((IkReal(0.840000000000000))*(sj5)); -IkReal x323=((npz)*(sj5)); -IkReal x324=((npy)*(x315)); -IkReal x325=((npx)*(x315)); -IkReal x326=((npy)*(x316)); -IkReal x327=((npx)*(x316)); -evalcond[0]=((((sj4)*(x320)))+(x324)+(x327)); -evalcond[1]=((IkReal(0.0637000000000000))+(((x322)*(x325)))+(((IkReal(-0.840000000000000))*(x318)))+(((IkReal(-1.00000000000000))*(x322)*(x326)))+(((IkReal(-1.00000000000000))*(pp)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x321)))+(((sj5)*(x325)))+(((IkReal(-1.00000000000000))*(sj5)*(x326)))+(((IkReal(-1.00000000000000))*(x318)))); -evalcond[3]=((((IkReal(-0.420000000000000))*(sj5)))+(((IkReal(-1.00000000000000))*(x326)))+(x325)+(((IkReal(-1.00000000000000))*(x319)*(x320)))+(((IkReal(-1.00000000000000))*(sj5)*(x321)))); -evalcond[4]=((((cj5)*(sj4)*(x325)))+(((sj4)*(x323)))+(((IkReal(-1.00000000000000))*(cj5)*(x317)*(x326)))+(((cj4)*(x324)))+(((cj4)*(x327)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x317)*(x327)))+(((IkReal(-1.00000000000000))*(x317)*(x324)))+(((IkReal(-1.00000000000000))*(x320)))+(((cj4)*(x323)))+(((IkReal(-1.00000000000000))*(x319)*(x326)))+(((x319)*(x325)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j6array[2], cj6array[2], sj6array[2]; -bool j6valid[2]={false}; -_nj6 = 2; -if( IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x328=((IkReal(1.00000000000000))*(IKatan2(npy, npx))); -if( ((((npx)*(npx))+((npy)*(npy)))) < (IkReal)-0.00001 ) - continue; -if( (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x329=IKasin(((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))); -j6array[0]=((((IkReal(-1.00000000000000))*(x328)))+(((IkReal(-1.00000000000000))*(x329)))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -j6array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x328)))+(x329)); -sj6array[1]=IKsin(j6array[1]); -cj6array[1]=IKcos(j6array[1]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -if( j6array[1] > IKPI ) -{ - j6array[1]-=IK2PI; -} -else if( j6array[1] < -IKPI ) -{ j6array[1]+=IK2PI; -} -j6valid[1] = true; -for(int ij6 = 0; ij6 < 2; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 2; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; -{ -IkReal evalcond[2]; -IkReal x330=(npx)*(npx); -IkReal x331=(cj4)*(cj4); -IkReal x332=(sj4)*(sj4); -IkReal x333=IKcos(j6); -IkReal x334=(npy)*(npy); -IkReal x335=IKsin(j6); -IkReal x336=((npx)*(npy)); -IkReal x337=((IkReal(0.490000000000000))*(sj3)*(sj4)); -IkReal x338=((IkReal(1.00000000000000))*(x334)); -evalcond[0]=((((x333)*(((((x331)*(x336)))+(((x332)*(x336)))))))+(((x335)*(((((x330)*(x331)))+(((x330)*(x332)))))))+(((npx)*(x337)))); -evalcond[1]=((((x335)*(((((IkReal(-1.00000000000000))*(x332)*(x336)))+(((IkReal(-1.00000000000000))*(x331)*(x336)))))))+(((x333)*(((((IkReal(-1.00000000000000))*(x331)*(x338)))+(((IkReal(-1.00000000000000))*(x332)*(x338)))))))+(((IkReal(-1.00000000000000))*(npy)*(x337)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst10; -IkReal x339=((cj6)*(npx)); -IkReal x340=((IkReal(49.0000000000000))*(cj3)); -IkReal x341=((npy)*(sj6)); -gconst10=IKsign(((((IkReal(-42.0000000000000))*(x339)))+(((IkReal(49.0000000000000))*(cj4)*(npz)*(sj3)))+(((x340)*(x341)))+(((IkReal(42.0000000000000))*(x341)))+(((IkReal(-1.00000000000000))*(x339)*(x340))))); -IkReal x342=((npy)*(sj6)); -IkReal x343=((cj6)*(npx)); -IkReal x344=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(x342)+(((IkReal(-1.00000000000000))*(x343)*(x344)))+(((IkReal(-1.00000000000000))*(x343)))+(((x342)*(x344)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst11; -IkReal x345=((npy)*(sj6)); -IkReal x346=((IkReal(1029.00000000000))*(cj3)); -IkReal x347=((cj6)*(npx)); -gconst11=IKsign(((((x345)*(x346)))+(((IkReal(1029.00000000000))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x346)*(x347)))+(((IkReal(-882.000000000000))*(x347)))+(((IkReal(882.000000000000))*(x345))))); -IkReal x348=((npy)*(sj6)); -IkReal x349=((cj6)*(npx)); -IkReal x350=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x349)*(x350)))+(x348)+(((x348)*(x350)))+(((IkReal(-1.00000000000000))*(x349)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x351=((cj4)*(sj3)); -IkReal x352=((IkReal(1225.00000000000))*(pp)); -IkReal x353=((IkReal(2100.00000000000))*(npz)); -if( IKabs(((gconst11)*(((IkReal(66.8850000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x352)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(78.0325000000000))*(cj3)))+(((npz)*(x353))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((IkReal(78.0325000000000))*(x351)))+(((cj6)*(npx)*(x353)))+(((IkReal(-1.00000000000000))*(x351)*(x352)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x353))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst11)*(((IkReal(66.8850000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x352)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(78.0325000000000))*(cj3)))+(((npz)*(x353)))))), ((gconst11)*(((((IkReal(78.0325000000000))*(x351)))+(((cj6)*(npx)*(x353)))+(((IkReal(-1.00000000000000))*(x351)*(x352)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(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[6]; -IkReal x354=IKcos(j5); -IkReal x355=IKsin(j5); -IkReal x356=((IkReal(1.00000000000000))*(npy)); -IkReal x357=((cj6)*(sj4)); -IkReal x358=((IkReal(0.490000000000000))*(sj3)); -IkReal x359=((sj4)*(sj6)); -IkReal x360=((IkReal(1.00000000000000))*(npz)); -IkReal x361=((cj6)*(npx)); -IkReal x362=((IkReal(0.490000000000000))*(cj3)); -IkReal x363=((npz)*(x355)); -IkReal x364=((cj4)*(x354)); -IkReal x365=((sj6)*(x355)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x354)*(x362)))+(((IkReal(-0.420000000000000))*(x354)))+(((IkReal(-1.00000000000000))*(x360)))+(((cj4)*(x355)*(x358)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(-0.840000000000000))*(npz)*(x354)))+(((IkReal(0.840000000000000))*(x355)*(x361)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x365)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x354)*(x360)))+(((IkReal(-1.00000000000000))*(x362)))+(((x355)*(x361)))+(((IkReal(-1.00000000000000))*(x356)*(x365)))); -evalcond[3]=((((IkReal(-0.420000000000000))*(x355)))+(((IkReal(-1.00000000000000))*(x358)*(x364)))+(((IkReal(-1.00000000000000))*(x355)*(x362)))+(x361)+(((IkReal(-1.00000000000000))*(sj6)*(x356)))); -evalcond[4]=((((sj4)*(x363)))+(((npx)*(x354)*(x357)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))+(((IkReal(-1.00000000000000))*(x354)*(x356)*(x359)))); -evalcond[5]=((((x361)*(x364)))+(((IkReal(-1.00000000000000))*(x358)))+(((IkReal(-1.00000000000000))*(x356)*(x357)))+(((cj4)*(x363)))+(((IkReal(-1.00000000000000))*(npx)*(x359)))+(((IkReal(-1.00000000000000))*(sj6)*(x356)*(x364)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x366=((cj4)*(sj3)); -IkReal x367=((IkReal(100.000000000000))*(npz)); -if( IKabs(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x367)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-20.5800000000000))*(x366)))+(((cj6)*(npx)*(x367)))+(((IkReal(-24.0100000000000))*(cj3)*(x366)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x367))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x367)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3))))))), ((gconst10)*(((((IkReal(-20.5800000000000))*(x366)))+(((cj6)*(npx)*(x367)))+(((IkReal(-24.0100000000000))*(cj3)*(x366)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x367))))))); -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 x368=IKcos(j5); -IkReal x369=IKsin(j5); -IkReal x370=((IkReal(1.00000000000000))*(npy)); -IkReal x371=((cj6)*(sj4)); -IkReal x372=((IkReal(0.490000000000000))*(sj3)); -IkReal x373=((sj4)*(sj6)); -IkReal x374=((IkReal(1.00000000000000))*(npz)); -IkReal x375=((cj6)*(npx)); -IkReal x376=((IkReal(0.490000000000000))*(cj3)); -IkReal x377=((npz)*(x369)); -IkReal x378=((cj4)*(x368)); -IkReal x379=((sj6)*(x369)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x368)*(x376)))+(((cj4)*(x369)*(x372)))+(((IkReal(-1.00000000000000))*(x374)))+(((IkReal(-0.420000000000000))*(x368)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(x369)*(x375)))+(((IkReal(-0.840000000000000))*(npz)*(x368)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x379)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x368)*(x374)))+(((IkReal(-1.00000000000000))*(x370)*(x379)))+(((x369)*(x375)))+(((IkReal(-1.00000000000000))*(x376)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x372)*(x378)))+(x375)+(((IkReal(-0.420000000000000))*(x369)))+(((IkReal(-1.00000000000000))*(sj6)*(x370)))+(((IkReal(-1.00000000000000))*(x369)*(x376)))); -evalcond[4]=((((sj4)*(x377)))+(((IkReal(-1.00000000000000))*(x368)*(x370)*(x373)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))+(((npx)*(x368)*(x371)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(npx)*(x373)))+(((x375)*(x378)))+(((IkReal(-1.00000000000000))*(x370)*(x371)))+(((IkReal(-1.00000000000000))*(x372)))+(((cj4)*(x377)))+(((IkReal(-1.00000000000000))*(sj6)*(x370)*(x378)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j6array[2], cj6array[2], sj6array[2]; -bool j6valid[2]={false}; -_nj6 = 2; -if( IKabs(npy) < IKFAST_ATAN2_MAGTHRESH && IKabs(npx) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x380=((IkReal(1.00000000000000))*(IKatan2(npy, npx))); -if( ((((npx)*(npx))+((npy)*(npy)))) < (IkReal)-0.00001 ) - continue; -if( (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH ) - continue; -IkReal x381=IKasin(((IkReal(0.490000000000000))*(sj3)*(sj4)*(((IKabs(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((npx)*(npx))+((npy)*(npy))))))):(IkReal)1.0e30)))); -j6array[0]=((((IkReal(-1.00000000000000))*(x381)))+(((IkReal(-1.00000000000000))*(x380)))); -sj6array[0]=IKsin(j6array[0]); -cj6array[0]=IKcos(j6array[0]); -j6array[1]=((IkReal(3.14159265358979))+(x381)+(((IkReal(-1.00000000000000))*(x380)))); -sj6array[1]=IKsin(j6array[1]); -cj6array[1]=IKcos(j6array[1]); -if( j6array[0] > IKPI ) -{ - j6array[0]-=IK2PI; -} -else if( j6array[0] < -IKPI ) -{ j6array[0]+=IK2PI; -} -j6valid[0] = true; -if( j6array[1] > IKPI ) -{ - j6array[1]-=IK2PI; -} -else if( j6array[1] < -IKPI ) -{ j6array[1]+=IK2PI; -} -j6valid[1] = true; -for(int ij6 = 0; ij6 < 2; ++ij6) -{ -if( !j6valid[ij6] ) -{ - continue; -} -_ij6[0] = ij6; _ij6[1] = -1; -for(int iij6 = ij6+1; iij6 < 2; ++iij6) -{ -if( j6valid[iij6] && IKabs(cj6array[ij6]-cj6array[iij6]) < IKFAST_SOLUTION_THRESH && IKabs(sj6array[ij6]-sj6array[iij6]) < IKFAST_SOLUTION_THRESH ) -{ - j6valid[iij6]=false; _ij6[1] = iij6; break; -} -} -j6 = j6array[ij6]; cj6 = cj6array[ij6]; sj6 = sj6array[ij6]; - -{ -IkReal dummyeval[1]; -IkReal gconst10; -IkReal x382=((cj6)*(npx)); -IkReal x383=((IkReal(49.0000000000000))*(cj3)); -IkReal x384=((npy)*(sj6)); -gconst10=IKsign(((((IkReal(-42.0000000000000))*(x382)))+(((IkReal(42.0000000000000))*(x384)))+(((x383)*(x384)))+(((IkReal(49.0000000000000))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x382)*(x383))))); -IkReal x385=((npy)*(sj6)); -IkReal x386=((cj6)*(npx)); -IkReal x387=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(x385)+(((IkReal(-1.00000000000000))*(x386)))+(((IkReal(-1.00000000000000))*(x386)*(x387)))+(((x385)*(x387)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst11; -IkReal x388=((npy)*(sj6)); -IkReal x389=((IkReal(1029.00000000000))*(cj3)); -IkReal x390=((cj6)*(npx)); -gconst11=IKsign(((((IkReal(-1.00000000000000))*(x389)*(x390)))+(((IkReal(-882.000000000000))*(x390)))+(((x388)*(x389)))+(((IkReal(1029.00000000000))*(cj4)*(npz)*(sj3)))+(((IkReal(882.000000000000))*(x388))))); -IkReal x391=((npy)*(sj6)); -IkReal x392=((cj6)*(npx)); -IkReal x393=((IkReal(1.16666666666667))*(cj3)); -dummyeval[0]=((((IkReal(1.16666666666667))*(cj4)*(npz)*(sj3)))+(((IkReal(-1.00000000000000))*(x392)))+(x391)+(((x391)*(x393)))+(((IkReal(-1.00000000000000))*(x392)*(x393)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x394=((cj4)*(sj3)); -IkReal x395=((IkReal(1225.00000000000))*(pp)); -IkReal x396=((IkReal(2100.00000000000))*(npz)); -if( IKabs(((gconst11)*(((IkReal(66.8850000000000))+(((npz)*(x396)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj3)*(x395)))+(((IkReal(78.0325000000000))*(cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj6)*(npx)*(x396)))+(((IkReal(-1.00000000000000))*(x394)*(x395)))+(((IkReal(78.0325000000000))*(x394)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x396))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst11)*(((IkReal(66.8850000000000))+(((npz)*(x396)))+(((IkReal(-1050.00000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj3)*(x395)))+(((IkReal(78.0325000000000))*(cj3)))))), ((gconst11)*(((((cj6)*(npx)*(x396)))+(((IkReal(-1.00000000000000))*(x394)*(x395)))+(((IkReal(78.0325000000000))*(x394)))+(((IkReal(-1.00000000000000))*(npy)*(sj6)*(x396))))))); -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 x397=IKcos(j5); -IkReal x398=IKsin(j5); -IkReal x399=((IkReal(1.00000000000000))*(npy)); -IkReal x400=((cj6)*(sj4)); -IkReal x401=((IkReal(0.490000000000000))*(sj3)); -IkReal x402=((sj4)*(sj6)); -IkReal x403=((IkReal(1.00000000000000))*(npz)); -IkReal x404=((cj6)*(npx)); -IkReal x405=((IkReal(0.490000000000000))*(cj3)); -IkReal x406=((npz)*(x398)); -IkReal x407=((cj4)*(x397)); -IkReal x408=((sj6)*(x398)); -evalcond[0]=((((cj4)*(x398)*(x401)))+(((IkReal(-1.00000000000000))*(x397)*(x405)))+(((IkReal(-0.420000000000000))*(x397)))+(((IkReal(-1.00000000000000))*(x403)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(x398)*(x404)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x408)))+(((IkReal(-0.840000000000000))*(npz)*(x397)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x399)*(x408)))+(((IkReal(-1.00000000000000))*(x397)*(x403)))+(((x398)*(x404)))+(((IkReal(-1.00000000000000))*(x405)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x401)*(x407)))+(((IkReal(-1.00000000000000))*(sj6)*(x399)))+(x404)+(((IkReal(-1.00000000000000))*(x398)*(x405)))+(((IkReal(-0.420000000000000))*(x398)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x397)*(x399)*(x402)))+(((npx)*(x397)*(x400)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))+(((sj4)*(x406)))); -evalcond[5]=((((x404)*(x407)))+(((IkReal(-1.00000000000000))*(sj6)*(x399)*(x407)))+(((cj4)*(x406)))+(((IkReal(-1.00000000000000))*(x399)*(x400)))+(((IkReal(-1.00000000000000))*(npx)*(x402)))+(((IkReal(-1.00000000000000))*(x401)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} - -} else -{ -{ -IkReal j5array[1], cj5array[1], sj5array[1]; -bool j5valid[1]={false}; -_nj5 = 1; -IkReal x409=((cj4)*(sj3)); -IkReal x410=((IkReal(100.000000000000))*(npz)); -if( IKabs(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x410)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3)))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(npy)*(sj6)*(x410)))+(((cj6)*(npx)*(x410)))+(((IkReal(-24.0100000000000))*(cj3)*(x409)))+(((IkReal(-20.5800000000000))*(x409))))))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j5array[0]=IKatan2(((gconst10)*(((IkReal(-17.6400000000000))+(((IkReal(-41.1600000000000))*(cj3)))+(((npz)*(x410)))+(((IkReal(-24.0100000000000))*((cj3)*(cj3))))))), ((gconst10)*(((((IkReal(-1.00000000000000))*(npy)*(sj6)*(x410)))+(((cj6)*(npx)*(x410)))+(((IkReal(-24.0100000000000))*(cj3)*(x409)))+(((IkReal(-20.5800000000000))*(x409))))))); -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 x411=IKcos(j5); -IkReal x412=IKsin(j5); -IkReal x413=((IkReal(1.00000000000000))*(npy)); -IkReal x414=((cj6)*(sj4)); -IkReal x415=((IkReal(0.490000000000000))*(sj3)); -IkReal x416=((sj4)*(sj6)); -IkReal x417=((IkReal(1.00000000000000))*(npz)); -IkReal x418=((cj6)*(npx)); -IkReal x419=((IkReal(0.490000000000000))*(cj3)); -IkReal x420=((npz)*(x412)); -IkReal x421=((cj4)*(x411)); -IkReal x422=((sj6)*(x412)); -evalcond[0]=((((cj4)*(x412)*(x415)))+(((IkReal(-0.420000000000000))*(x411)))+(((IkReal(-1.00000000000000))*(x411)*(x419)))+(((IkReal(-1.00000000000000))*(x417)))); -evalcond[1]=((IkReal(0.0637000000000000))+(((IkReal(0.840000000000000))*(x412)*(x418)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.840000000000000))*(npy)*(x422)))+(((IkReal(-0.840000000000000))*(npz)*(x411)))); -evalcond[2]=((IkReal(-0.420000000000000))+(((IkReal(-1.00000000000000))*(x411)*(x417)))+(((x412)*(x418)))+(((IkReal(-1.00000000000000))*(x413)*(x422)))+(((IkReal(-1.00000000000000))*(x419)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(sj6)*(x413)))+(((IkReal(-1.00000000000000))*(x412)*(x419)))+(((IkReal(-0.420000000000000))*(x412)))+(x418)+(((IkReal(-1.00000000000000))*(x415)*(x421)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x411)*(x413)*(x416)))+(((npx)*(x411)*(x414)))+(((sj4)*(x420)))+(((cj4)*(npx)*(sj6)))+(((cj4)*(cj6)*(npy)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(sj6)*(x413)*(x421)))+(((x418)*(x421)))+(((cj4)*(x420)))+(((IkReal(-1.00000000000000))*(x413)*(x414)))+(((IkReal(-1.00000000000000))*(npx)*(x416)))+(((IkReal(-1.00000000000000))*(x415)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -rotationfunction0(solutions); -} -} - -} - -} -} -} - -} - -} -} -} -} -return solutions.GetNumSolutions()>0; -} -inline void rotationfunction0(IkSolutionListBase& solutions) { -for(int rotationiter = 0; rotationiter < 1; ++rotationiter) { -IkReal x78=((cj6)*(sj4)); -IkReal x79=((IkReal(1.00000000000000))*(sj5)); -IkReal x80=((cj4)*(cj5)); -IkReal x81=((IkReal(1.00000000000000))*(sj3)); -IkReal x82=((cj3)*(sj5)); -IkReal x83=((IkReal(1.00000000000000))*(sj6)); -IkReal x84=((sj4)*(sj6)); -IkReal x85=((IkReal(1.00000000000000))*(cj3)); -IkReal x86=((IkReal(-1.00000000000000))*(cj3)); -IkReal x87=((((cj3)*(x80)))+(((IkReal(-1.00000000000000))*(sj3)*(x79)))); -IkReal x88=((((cj5)*(sj3)))+(((cj4)*(x82)))); -IkReal x89=((((cj5)*(x84)))+(((IkReal(-1.00000000000000))*(cj4)*(cj6)))); -IkReal x90=((x82)+(((sj3)*(x80)))); -IkReal x91=((((cj4)*(sj3)*(sj5)))+(((IkReal(-1.00000000000000))*(cj5)*(x85)))); -IkReal x92=((cj6)*(x87)); -IkReal x93=((((IkReal(-1.00000000000000))*(cj4)*(x83)))+(((IkReal(-1.00000000000000))*(cj5)*(x78)))); -IkReal x94=((((IkReal(-1.00000000000000))*(cj3)*(sj4)*(x83)))+(x92)); -IkReal x95=((((IkReal(-1.00000000000000))*(x81)*(x84)))+(((cj6)*(x90)))); -IkReal x96=((((IkReal(-1.00000000000000))*(x83)*(x90)))+(((IkReal(-1.00000000000000))*(x78)*(x81)))); -IkReal x97=((((x78)*(x86)))+(((IkReal(-1.00000000000000))*(sj6)*(x87)))); -new_r00=((((r00)*(((((x84)*(x86)))+(x92)))))+(((r01)*(((((IkReal(-1.00000000000000))*(x83)*(x87)))+(((IkReal(-1.00000000000000))*(x78)*(x85)))))))+(((r02)*(x88)))); -new_r01=((((r12)*(x88)))+(((r11)*(x97)))+(((r10)*(x94)))); -new_r02=((((r21)*(x97)))+(((r20)*(x94)))+(((r22)*(x88)))); -new_r10=((((r00)*(x93)))+(((r01)*(x89)))+(((IkReal(-1.00000000000000))*(r02)*(sj4)*(x79)))); -new_r11=((((r11)*(x89)))+(((r10)*(x93)))+(((IkReal(-1.00000000000000))*(r12)*(sj4)*(x79)))); -new_r12=((((r21)*(x89)))+(((IkReal(-1.00000000000000))*(r22)*(sj4)*(x79)))+(((r20)*(x93)))); -new_r20=((((r01)*(x96)))+(((r00)*(x95)))+(((r02)*(x91)))); -new_r21=((((r12)*(x91)))+(((r11)*(x96)))+(((r10)*(x95)))); -new_r22=((((r21)*(x96)))+(((r20)*(x95)))+(((r22)*(x91)))); -{ -IkReal j1array[2], cj1array[2], sj1array[2]; -bool j1valid[2]={false}; -_nj1 = 2; -cj1array[0]=new_r22; -if( cj1array[0] >= -1-IKFAST_SINCOS_THRESH && cj1array[0] <= 1+IKFAST_SINCOS_THRESH ) -{ - j1valid[0] = j1valid[1] = true; - j1array[0] = IKacos(cj1array[0]); - sj1array[0] = IKsin(j1array[0]); - cj1array[1] = cj1array[0]; - j1array[1] = -j1array[0]; - sj1array[1] = -sj1array[0]; -} -else if( std::isnan(cj1array[0]) ) -{ - // probably any value will work - j1valid[0] = true; - cj1array[0] = 1; sj1array[0] = 0; j1array[0] = 0; -} -for(int ij1 = 0; ij1 < 2; ++ij1) -{ -if( !j1valid[ij1] ) -{ - continue; -} -_ij1[0] = ij1; _ij1[1] = -1; -for(int iij1 = ij1+1; iij1 < 2; ++iij1) -{ -if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH ) -{ - j1valid[iij1]=false; _ij1[1] = iij1; break; -} -} -j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1]; - -{ -IkReal dummyeval[1]; -IkReal gconst12; -gconst12=IKsign(sj1); -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst13; -gconst13=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst14; -gconst14=IKsign(((((cj1)*((new_r12)*(new_r12))))+(((cj1)*((new_r02)*(new_r02)))))); -dummyeval[0]=((((cj1)*((new_r12)*(new_r12))))+(((cj1)*((new_r02)*(new_r02))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[7]; -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=new_r22; -evalcond[2]=new_r22; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(new_r21, new_r20); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[2]; -evalcond[0]=((((IkReal(-1.00000000000000))*(IKcos(j0))))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(IKsin(j0))))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst21; -gconst21=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst20; -gconst20=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x98=((gconst20)*(sj0)); -if( IKabs(((new_r12)*(x98))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x98))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x98)), ((IkReal(-1.00000000000000))*(new_r02)*(x98))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x99=IKsin(j2); -IkReal x100=IKcos(j2); -IkReal x101=((IkReal(1.00000000000000))*(x99)); -evalcond[0]=((((new_r02)*(x99)))+(((new_r12)*(x100)))); -evalcond[1]=((((new_r10)*(x100)))+(sj0)+(((new_r00)*(x99)))); -evalcond[2]=((IkReal(1.00000000000000))+(((new_r02)*(x100)))+(((IkReal(-1.00000000000000))*(new_r12)*(x101)))); -evalcond[3]=((((new_r01)*(x99)))+(((IkReal(-1.00000000000000))*(cj0)))+(((new_r11)*(x100)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x101)))+(((new_r00)*(x100)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x101)))+(((new_r01)*(x100)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -if( IKabs(((gconst21)*(new_r12))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst21)*(new_r02))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst21)*(new_r12)), ((IkReal(-1.00000000000000))*(gconst21)*(new_r02))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x102=IKsin(j2); -IkReal x103=IKcos(j2); -IkReal x104=((IkReal(1.00000000000000))*(x102)); -evalcond[0]=((((new_r02)*(x102)))+(((new_r12)*(x103)))); -evalcond[1]=((((new_r10)*(x103)))+(sj0)+(((new_r00)*(x102)))); -evalcond[2]=((IkReal(1.00000000000000))+(((new_r02)*(x103)))+(((IkReal(-1.00000000000000))*(new_r12)*(x104)))); -evalcond[3]=((((new_r01)*(x102)))+(((IkReal(-1.00000000000000))*(cj0)))+(((new_r11)*(x103)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x104)))+(((new_r00)*(x103)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x104)))+(((new_r01)*(x103)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} else -{ -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=new_r22; -evalcond[2]=((IkReal(-1.00000000000000))*(new_r22)); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((IkReal(-1.00000000000000))*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)))+IKsqr(((IkReal(-1.00000000000000))*(new_r20)))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)), ((IkReal(-1.00000000000000))*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[2]; -evalcond[0]=((IKcos(j0))+(new_r20)); -evalcond[1]=((IKsin(j0))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst24; -gconst24=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst25; -gconst25=IKsign(((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12)))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12))))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -if( IKabs(((gconst25)*(new_r12))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst25)*(new_r02))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((gconst25)*(new_r12)), ((IkReal(-1.00000000000000))*(gconst25)*(new_r02))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x105=IKsin(j2); -IkReal x106=IKcos(j2); -IkReal x107=((IkReal(1.00000000000000))*(x105)); -evalcond[0]=((((new_r02)*(x105)))+(((new_r12)*(x106)))); -evalcond[1]=((((new_r10)*(x106)))+(sj0)+(((new_r00)*(x105)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((new_r02)*(x106)))+(((IkReal(-1.00000000000000))*(new_r12)*(x107)))); -evalcond[3]=((((new_r01)*(x105)))+(((IkReal(-1.00000000000000))*(cj0)))+(((new_r11)*(x106)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x107)))+(((new_r00)*(x106)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x107)))+(((new_r01)*(x106)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x108=((gconst24)*(sj0)); -if( IKabs(((new_r12)*(x108))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x108))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x108)), ((IkReal(-1.00000000000000))*(new_r02)*(x108))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x109=IKsin(j2); -IkReal x110=IKcos(j2); -IkReal x111=((IkReal(1.00000000000000))*(x109)); -evalcond[0]=((((new_r02)*(x109)))+(((new_r12)*(x110)))); -evalcond[1]=((((new_r10)*(x110)))+(sj0)+(((new_r00)*(x109)))); -evalcond[2]=((IkReal(-1.00000000000000))+(((new_r02)*(x110)))+(((IkReal(-1.00000000000000))*(new_r12)*(x111)))); -evalcond[3]=((((new_r11)*(x110)))+(((new_r01)*(x109)))+(((IkReal(-1.00000000000000))*(cj0)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(new_r10)*(x111)))+(((new_r00)*(x110)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x111)))+(((new_r01)*(x110)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} else -{ -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[5]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[6]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 ) -{ -{ -IkReal j2array[2], cj2array[2], sj2array[2]; -bool j2valid[2]={false}; -_nj2 = 2; -if( IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x112=IKatan2(new_r12, new_r02); -j2array[0]=((IkReal(-1.00000000000000))*(x112)); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -j2array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x112)))); -sj2array[1]=IKsin(j2array[1]); -cj2array[1]=IKcos(j2array[1]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -if( j2array[1] > IKPI ) -{ - j2array[1]-=IK2PI; -} -else if( j2array[1] < -IKPI ) -{ j2array[1]+=IK2PI; -} -j2valid[1] = true; -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 evalcond[1]; -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r12)*(IKsin(j2))))+(((new_r02)*(IKcos(j2))))); -if( IKabs(evalcond[0]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x113=IKsin(j0); -IkReal x114=((IkReal(1.00000000000000))*(sj2)); -IkReal x115=((IkReal(1.00000000000000))*(IKcos(j0))); -evalcond[0]=((((new_r00)*(sj2)))+(x113)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x115)))+(((cj2)*(new_r11)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x114)))+(((IkReal(-1.00000000000000))*(x115)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x114)))+(((cj2)*(new_r01)))+(((IkReal(-1.00000000000000))*(x113)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} -} -} - -} else -{ -IkReal x116=((IkReal(1.00000000000000))+(new_r22)); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=x116; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=new_r20; -evalcond[5]=new_r21; -evalcond[6]=x116; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 ) -{ -{ -IkReal j2array[2], cj2array[2], sj2array[2]; -bool j2valid[2]={false}; -_nj2 = 2; -if( IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH ) - continue; -IkReal x117=IKatan2(new_r12, new_r02); -j2array[0]=((IkReal(-1.00000000000000))*(x117)); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -j2array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x117)))); -sj2array[1]=IKsin(j2array[1]); -cj2array[1]=IKcos(j2array[1]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -if( j2array[1] > IKPI ) -{ - j2array[1]-=IK2PI; -} -else if( j2array[1] < -IKPI ) -{ j2array[1]+=IK2PI; -} -j2valid[1] = true; -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 evalcond[1]; -evalcond[0]=((((IkReal(-1.00000000000000))*(new_r12)*(IKsin(j2))))+(((new_r02)*(IKcos(j2))))); -if( IKabs(evalcond[0]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x118=IKcos(j0); -IkReal x119=IKsin(j0); -IkReal x120=((IkReal(1.00000000000000))*(sj2)); -evalcond[0]=((((new_r00)*(sj2)))+(x119)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((cj2)*(new_r11)))+(((IkReal(-1.00000000000000))*(x118)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x120)))+(x118)+(((cj2)*(new_r00)))); -evalcond[3]=((x119)+(((cj2)*(new_r01)))+(((IkReal(-1.00000000000000))*(new_r11)*(x120)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} -} -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} -} -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x121=((gconst14)*(new_r22)*(sj1)); -if( IKabs(((new_r12)*(x121))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x121))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x121)), ((IkReal(-1.00000000000000))*(new_r02)*(x121))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x122=IKcos(j2); -IkReal x123=IKsin(j2); -IkReal x124=((IkReal(1.00000000000000))*(cj1)); -IkReal x125=((sj1)*(x122)); -IkReal x126=((new_r12)*(x123)); -IkReal x127=((new_r02)*(x122)); -IkReal x128=((IkReal(1.00000000000000))*(sj1)*(x123)); -evalcond[0]=((((new_r02)*(x123)))+(((new_r12)*(x122)))); -evalcond[1]=((sj1)+(x127)+(((IkReal(-1.00000000000000))*(x126)))); -evalcond[2]=((((cj1)*(x127)))+(((IkReal(-1.00000000000000))*(x124)*(x126)))+(((new_r22)*(sj1)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r10)*(x128)))+(((new_r00)*(x125)))+(((IkReal(-1.00000000000000))*(new_r20)*(x124)))); -evalcond[4]=((((new_r01)*(x125)))+(((IkReal(-1.00000000000000))*(new_r21)*(x124)))+(((IkReal(-1.00000000000000))*(new_r11)*(x128)))); -evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x126)))+(((new_r02)*(x125)))+(((IkReal(-1.00000000000000))*(new_r22)*(x124)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst15; -gconst15=IKsign(sj1); -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x129=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -IkReal x130=((((cj2)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj2)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x129; -evalcond[5]=x129; -evalcond[6]=x130; -evalcond[7]=x130; -evalcond[8]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[9]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x131=IKsin(j0); -IkReal x132=((IkReal(1.00000000000000))*(sj2)); -IkReal x133=((IkReal(1.00000000000000))*(IKcos(j0))); -evalcond[0]=((((new_r00)*(sj2)))+(x131)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x133)))+(((cj2)*(new_r11)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x132)))+(((IkReal(-1.00000000000000))*(x133)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x132)))+(((cj2)*(new_r01)))+(((IkReal(-1.00000000000000))*(x131)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -IkReal x134=((IkReal(1.00000000000000))+(new_r22)); -IkReal x135=((new_r12)*(sj2)); -IkReal x136=((cj2)*(new_r02)); -IkReal x137=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=x134; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x137; -evalcond[5]=x137; -evalcond[6]=((x136)+(((IkReal(-1.00000000000000))*(x135)))); -evalcond[7]=((x135)+(((IkReal(-1.00000000000000))*(x136)))); -evalcond[8]=new_r20; -evalcond[9]=new_r21; -evalcond[10]=x134; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x138=IKcos(j0); -IkReal x139=IKsin(j0); -IkReal x140=((IkReal(1.00000000000000))*(sj2)); -evalcond[0]=((((new_r00)*(sj2)))+(x139)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((cj2)*(new_r11)))+(((IkReal(-1.00000000000000))*(x138)))); -evalcond[2]=((x138)+(((IkReal(-1.00000000000000))*(new_r10)*(x140)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x140)))+(x139)+(((cj2)*(new_r01)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x141=IKsin(j0); -IkReal x142=IKcos(j0); -IkReal x143=((cj2)*(new_r01)); -IkReal x144=((IkReal(1.00000000000000))*(cj1)); -IkReal x145=((IkReal(1.00000000000000))*(sj1)); -IkReal x146=((new_r10)*(sj2)); -IkReal x147=((new_r11)*(sj2)); -IkReal x148=((cj2)*(new_r00)); -IkReal x149=((IkReal(1.00000000000000))*(x142)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x142)*(x145)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x141)*(x145)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x141)+(((cj2)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x149)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x142)*(x144)))+(x148)+(((IkReal(-1.00000000000000))*(x146)))); -evalcond[5]=((x143)+(((IkReal(-1.00000000000000))*(x147)))+(((IkReal(-1.00000000000000))*(x141)*(x144)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x149)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x144)*(x146)))+(((cj1)*(x148)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x144)*(x147)))+(((IkReal(-1.00000000000000))*(x141)))+(((new_r21)*(sj1)))+(((cj1)*(x143)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x150=IKsin(j0); -IkReal x151=IKcos(j0); -IkReal x152=((cj2)*(new_r01)); -IkReal x153=((IkReal(1.00000000000000))*(cj1)); -IkReal x154=((IkReal(1.00000000000000))*(sj1)); -IkReal x155=((new_r10)*(sj2)); -IkReal x156=((new_r11)*(sj2)); -IkReal x157=((cj2)*(new_r00)); -IkReal x158=((IkReal(1.00000000000000))*(x151)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x151)*(x154)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x150)*(x154)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x150)+(((cj2)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x158)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x151)*(x153)))+(x157)+(((IkReal(-1.00000000000000))*(x155)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x150)*(x153)))+(x152)+(((IkReal(-1.00000000000000))*(x156)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x158)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x153)*(x155)))+(((cj1)*(x157)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x153)*(x156)))+(((IkReal(-1.00000000000000))*(x150)))+(((new_r21)*(sj1)))+(((cj1)*(x152)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((gconst15)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j0array[0]=IKatan2(((gconst15)*(new_r21)), ((gconst15)*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x159=IKsin(j0); -IkReal x160=IKcos(j0); -IkReal x161=((cj2)*(new_r01)); -IkReal x162=((IkReal(1.00000000000000))*(cj1)); -IkReal x163=((IkReal(1.00000000000000))*(sj1)); -IkReal x164=((new_r10)*(sj2)); -IkReal x165=((new_r11)*(sj2)); -IkReal x166=((cj2)*(new_r00)); -IkReal x167=((IkReal(1.00000000000000))*(x160)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x160)*(x163)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x159)*(x163)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x159)+(((cj2)*(new_r10)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x167)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x160)*(x162)))+(x166)+(((IkReal(-1.00000000000000))*(x164)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x159)*(x162)))+(x161)+(((IkReal(-1.00000000000000))*(x165)))); -evalcond[6]=((((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x167)))+(((IkReal(-1.00000000000000))*(x162)*(x164)))+(((cj1)*(x166)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x162)*(x165)))+(((IkReal(-1.00000000000000))*(x159)))+(((cj1)*(x161)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x168=((gconst13)*(sj1)); -if( IKabs(((new_r12)*(x168))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x168))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x168)), ((IkReal(-1.00000000000000))*(new_r02)*(x168))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[6]; -IkReal x169=IKcos(j2); -IkReal x170=IKsin(j2); -IkReal x171=((IkReal(1.00000000000000))*(cj1)); -IkReal x172=((sj1)*(x169)); -IkReal x173=((new_r12)*(x170)); -IkReal x174=((new_r02)*(x169)); -IkReal x175=((IkReal(1.00000000000000))*(sj1)*(x170)); -evalcond[0]=((((new_r02)*(x170)))+(((new_r12)*(x169)))); -evalcond[1]=((sj1)+(((IkReal(-1.00000000000000))*(x173)))+(x174)); -evalcond[2]=((((new_r22)*(sj1)))+(((IkReal(-1.00000000000000))*(x171)*(x173)))+(((cj1)*(x174)))); -evalcond[3]=((((new_r00)*(x172)))+(((IkReal(-1.00000000000000))*(new_r20)*(x171)))+(((IkReal(-1.00000000000000))*(new_r10)*(x175)))); -evalcond[4]=((((new_r01)*(x172)))+(((IkReal(-1.00000000000000))*(new_r21)*(x171)))+(((IkReal(-1.00000000000000))*(new_r11)*(x175)))); -evalcond[5]=((IkReal(1.00000000000000))+(((new_r02)*(x172)))+(((IkReal(-1.00000000000000))*(sj1)*(x173)))+(((IkReal(-1.00000000000000))*(new_r22)*(x171)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst15; -gconst15=IKsign(sj1); -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -dummyeval[0]=sj1; -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal evalcond[11]; -IkReal x176=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -IkReal x177=((((cj2)*(new_r02)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj2)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=((IkReal(-1.00000000000000))+(new_r22)); -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x176; -evalcond[5]=x176; -evalcond[6]=x177; -evalcond[7]=x177; -evalcond[8]=((IkReal(-1.00000000000000))*(new_r20)); -evalcond[9]=((IkReal(-1.00000000000000))*(new_r21)); -evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)))); -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x178=IKsin(j0); -IkReal x179=((IkReal(1.00000000000000))*(sj2)); -IkReal x180=((IkReal(1.00000000000000))*(IKcos(j0))); -evalcond[0]=((((new_r00)*(sj2)))+(x178)+(((cj2)*(new_r10)))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x180)))+(((new_r01)*(sj2)))+(((cj2)*(new_r11)))); -evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x179)))+(((IkReal(-1.00000000000000))*(x180)))+(((cj2)*(new_r00)))); -evalcond[3]=((((IkReal(-1.00000000000000))*(x178)))+(((IkReal(-1.00000000000000))*(new_r11)*(x179)))+(((cj2)*(new_r01)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -IkReal x181=((IkReal(1.00000000000000))+(new_r22)); -IkReal x182=((new_r12)*(sj2)); -IkReal x183=((cj2)*(new_r02)); -IkReal x184=((((new_r02)*(sj2)))+(((cj2)*(new_r12)))); -evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.11022302462516e-16))+(j1)), IkReal(6.28318530717959)))); -evalcond[1]=x181; -evalcond[2]=new_r20; -evalcond[3]=new_r21; -evalcond[4]=x184; -evalcond[5]=x184; -evalcond[6]=((((IkReal(-1.00000000000000))*(x182)))+(x183)); -evalcond[7]=((((IkReal(-1.00000000000000))*(x183)))+(x182)); -evalcond[8]=new_r20; -evalcond[9]=new_r21; -evalcond[10]=x181; -if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 ) -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[4]; -IkReal x185=IKcos(j0); -IkReal x186=IKsin(j0); -IkReal x187=((IkReal(1.00000000000000))*(sj2)); -evalcond[0]=((((new_r00)*(sj2)))+(x186)+(((cj2)*(new_r10)))); -evalcond[1]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x185)))+(((cj2)*(new_r11)))); -evalcond[2]=((x185)+(((IkReal(-1.00000000000000))*(new_r10)*(x187)))+(((cj2)*(new_r00)))); -evalcond[3]=((x186)+(((IkReal(-1.00000000000000))*(new_r11)*(x187)))+(((cj2)*(new_r01)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} else -{ -if( 1 ) -{ -continue; - -} else -{ -} -} -} -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))))+IKsqr(((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((((IkReal(-1.00000000000000))*(cj2)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(sj2)))), ((new_r20)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x188=IKsin(j0); -IkReal x189=IKcos(j0); -IkReal x190=((cj2)*(new_r01)); -IkReal x191=((IkReal(1.00000000000000))*(cj1)); -IkReal x192=((IkReal(1.00000000000000))*(sj1)); -IkReal x193=((new_r10)*(sj2)); -IkReal x194=((new_r11)*(sj2)); -IkReal x195=((cj2)*(new_r00)); -IkReal x196=((IkReal(1.00000000000000))*(x189)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x189)*(x192)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x188)*(x192)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x188)+(((cj2)*(new_r10)))); -evalcond[3]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x196)))+(((cj2)*(new_r11)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x189)*(x191)))+(((IkReal(-1.00000000000000))*(x193)))+(x195)); -evalcond[5]=((((IkReal(-1.00000000000000))*(x188)*(x191)))+(((IkReal(-1.00000000000000))*(x194)))+(x190)); -evalcond[6]=((((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x191)*(x193)))+(((IkReal(-1.00000000000000))*(x196)))+(((cj1)*(x195)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x191)*(x194)))+(((IkReal(-1.00000000000000))*(x188)))+(((cj1)*(x190)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((new_r01)*(sj2)))+(((cj2)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))))+IKsqr(((((new_r01)*(sj2)))+(((cj2)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH ) - continue; -j0array[0]=IKatan2(((new_r21)*(((IKabs(sj1) != 0)?((IkReal)1/(sj1)):(IkReal)1.0e30))), ((((new_r01)*(sj2)))+(((cj2)*(new_r11))))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x197=IKsin(j0); -IkReal x198=IKcos(j0); -IkReal x199=((cj2)*(new_r01)); -IkReal x200=((IkReal(1.00000000000000))*(cj1)); -IkReal x201=((IkReal(1.00000000000000))*(sj1)); -IkReal x202=((new_r10)*(sj2)); -IkReal x203=((new_r11)*(sj2)); -IkReal x204=((cj2)*(new_r00)); -IkReal x205=((IkReal(1.00000000000000))*(x198)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x198)*(x201)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x197)*(x201)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x197)+(((cj2)*(new_r10)))); -evalcond[3]=((((new_r01)*(sj2)))+(((IkReal(-1.00000000000000))*(x205)))+(((cj2)*(new_r11)))); -evalcond[4]=((x204)+(((IkReal(-1.00000000000000))*(x198)*(x200)))+(((IkReal(-1.00000000000000))*(x202)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x197)*(x200)))+(x199)+(((IkReal(-1.00000000000000))*(x203)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(x200)*(x202)))+(((cj1)*(x204)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(x205)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(x200)*(x203)))+(((IkReal(-1.00000000000000))*(x197)))+(((cj1)*(x199)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((gconst15)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j0array[0]=IKatan2(((gconst15)*(new_r21)), ((gconst15)*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[8]; -IkReal x206=IKsin(j0); -IkReal x207=IKcos(j0); -IkReal x208=((cj2)*(new_r01)); -IkReal x209=((IkReal(1.00000000000000))*(cj1)); -IkReal x210=((IkReal(1.00000000000000))*(sj1)); -IkReal x211=((new_r10)*(sj2)); -IkReal x212=((new_r11)*(sj2)); -IkReal x213=((cj2)*(new_r00)); -IkReal x214=((IkReal(1.00000000000000))*(x207)); -evalcond[0]=((((IkReal(-1.00000000000000))*(x207)*(x210)))+(new_r20)); -evalcond[1]=((((IkReal(-1.00000000000000))*(x206)*(x210)))+(new_r21)); -evalcond[2]=((((new_r00)*(sj2)))+(x206)+(((cj2)*(new_r10)))); -evalcond[3]=((((new_r01)*(sj2)))+(((cj2)*(new_r11)))+(((IkReal(-1.00000000000000))*(x214)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(x207)*(x209)))+(x213)+(((IkReal(-1.00000000000000))*(x211)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(x206)*(x209)))+(x208)+(((IkReal(-1.00000000000000))*(x212)))); -evalcond[6]=((((new_r20)*(sj1)))+(((cj1)*(x213)))+(((IkReal(-1.00000000000000))*(x214)))+(((IkReal(-1.00000000000000))*(x209)*(x211)))); -evalcond[7]=((((cj1)*(x208)))+(((new_r21)*(sj1)))+(((IkReal(-1.00000000000000))*(x206)))+(((IkReal(-1.00000000000000))*(x209)*(x212)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} - -} else -{ -{ -IkReal j0array[1], cj0array[1], sj0array[1]; -bool j0valid[1]={false}; -_nj0 = 1; -if( IKabs(((gconst12)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j0array[0]=IKatan2(((gconst12)*(new_r21)), ((gconst12)*(new_r20))); -sj0array[0]=IKsin(j0array[0]); -cj0array[0]=IKcos(j0array[0]); -if( j0array[0] > IKPI ) -{ - j0array[0]-=IK2PI; -} -else if( j0array[0] < -IKPI ) -{ j0array[0]+=IK2PI; -} -j0valid[0] = true; -for(int ij0 = 0; ij0 < 1; ++ij0) -{ -if( !j0valid[ij0] ) -{ - continue; -} -_ij0[0] = ij0; _ij0[1] = -1; -for(int iij0 = ij0+1; iij0 < 1; ++iij0) -{ -if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH ) -{ - j0valid[iij0]=false; _ij0[1] = iij0; break; -} -} -j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0]; -{ -IkReal evalcond[2]; -IkReal x215=((IkReal(1.00000000000000))*(sj1)); -evalcond[0]=((new_r20)+(((IkReal(-1.00000000000000))*(x215)*(IKcos(j0))))); -evalcond[1]=((((IkReal(-1.00000000000000))*(x215)*(IKsin(j0))))+(new_r21)); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 ) -{ -continue; -} -} - -{ -IkReal dummyeval[1]; -IkReal gconst17; -gconst17=IKsign((((new_r12)*(new_r12))+((new_r02)*(new_r02)))); -dummyeval[0]=(((new_r12)*(new_r12))+((new_r02)*(new_r02))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -{ -IkReal dummyeval[1]; -IkReal gconst16; -gconst16=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))))); -dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))); -if( IKabs(dummyeval[0]) < 0.0000010000000000 ) -{ -continue; - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x216=((gconst16)*(sj0)); -if( IKabs(((new_r12)*(x216))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x216))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x216)), ((IkReal(-1.00000000000000))*(new_r02)*(x216))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[12]; -IkReal x217=IKsin(j2); -IkReal x218=IKcos(j2); -IkReal x219=((IkReal(1.00000000000000))*(cj0)); -IkReal x220=((IkReal(1.00000000000000))*(cj1)); -IkReal x221=((IkReal(1.00000000000000))*(x217)); -IkReal x222=((new_r01)*(x218)); -IkReal x223=((new_r00)*(x218)); -IkReal x224=((new_r02)*(x218)); -evalcond[0]=((((new_r02)*(x217)))+(((new_r12)*(x218)))); -evalcond[1]=((sj0)+(((new_r00)*(x217)))+(((new_r10)*(x218)))); -evalcond[2]=((sj1)+(x224)+(((IkReal(-1.00000000000000))*(new_r12)*(x221)))); -evalcond[3]=((((new_r01)*(x217)))+(((IkReal(-1.00000000000000))*(x219)))+(((new_r11)*(x218)))); -evalcond[4]=((((IkReal(-1.00000000000000))*(cj1)*(x219)))+(((IkReal(-1.00000000000000))*(new_r10)*(x221)))+(x223)); -evalcond[5]=((((IkReal(-1.00000000000000))*(new_r11)*(x221)))+(((IkReal(-1.00000000000000))*(sj0)*(x220)))+(x222)); -evalcond[6]=((((IkReal(-1.00000000000000))*(new_r12)*(x217)*(x220)))+(((cj1)*(x224)))+(((new_r22)*(sj1)))); -evalcond[7]=((((IkReal(-1.00000000000000))*(new_r10)*(sj1)*(x221)))+(((sj1)*(x223)))+(((IkReal(-1.00000000000000))*(new_r20)*(x220)))); -evalcond[8]=((((sj1)*(x222)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj1)*(x221)))+(((IkReal(-1.00000000000000))*(new_r21)*(x220)))); -evalcond[9]=((IkReal(1.00000000000000))+(((sj1)*(x224)))+(((IkReal(-1.00000000000000))*(new_r22)*(x220)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj1)*(x221)))); -evalcond[10]=((((new_r20)*(sj1)))+(((cj1)*(x223)))+(((IkReal(-1.00000000000000))*(new_r10)*(x217)*(x220)))+(((IkReal(-1.00000000000000))*(x219)))); -evalcond[11]=((((IkReal(-1.00000000000000))*(sj0)))+(((cj1)*(x222)))+(((IkReal(-1.00000000000000))*(new_r11)*(x217)*(x220)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} - -} else -{ -{ -IkReal j2array[1], cj2array[1], sj2array[1]; -bool j2valid[1]={false}; -_nj2 = 1; -IkReal x225=((gconst17)*(sj1)); -if( IKabs(((new_r12)*(x225))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r02)*(x225))) < IKFAST_ATAN2_MAGTHRESH ) - continue; -j2array[0]=IKatan2(((new_r12)*(x225)), ((IkReal(-1.00000000000000))*(new_r02)*(x225))); -sj2array[0]=IKsin(j2array[0]); -cj2array[0]=IKcos(j2array[0]); -if( j2array[0] > IKPI ) -{ - j2array[0]-=IK2PI; -} -else if( j2array[0] < -IKPI ) -{ j2array[0]+=IK2PI; -} -j2valid[0] = true; -for(int ij2 = 0; ij2 < 1; ++ij2) -{ -if( !j2valid[ij2] ) -{ - continue; -} -_ij2[0] = ij2; _ij2[1] = -1; -for(int iij2 = ij2+1; iij2 < 1; ++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 evalcond[12]; -IkReal x226=IKsin(j2); -IkReal x227=IKcos(j2); -IkReal x228=((IkReal(1.00000000000000))*(cj0)); -IkReal x229=((IkReal(1.00000000000000))*(cj1)); -IkReal x230=((IkReal(1.00000000000000))*(x226)); -IkReal x231=((new_r01)*(x227)); -IkReal x232=((new_r00)*(x227)); -IkReal x233=((new_r02)*(x227)); -evalcond[0]=((((new_r02)*(x226)))+(((new_r12)*(x227)))); -evalcond[1]=((((new_r00)*(x226)))+(sj0)+(((new_r10)*(x227)))); -evalcond[2]=((sj1)+(((IkReal(-1.00000000000000))*(new_r12)*(x230)))+(x233)); -evalcond[3]=((((new_r01)*(x226)))+(((new_r11)*(x227)))+(((IkReal(-1.00000000000000))*(x228)))); -evalcond[4]=((x232)+(((IkReal(-1.00000000000000))*(cj1)*(x228)))+(((IkReal(-1.00000000000000))*(new_r10)*(x230)))); -evalcond[5]=((((IkReal(-1.00000000000000))*(sj0)*(x229)))+(x231)+(((IkReal(-1.00000000000000))*(new_r11)*(x230)))); -evalcond[6]=((((IkReal(-1.00000000000000))*(new_r12)*(x226)*(x229)))+(((cj1)*(x233)))+(((new_r22)*(sj1)))); -evalcond[7]=((((sj1)*(x232)))+(((IkReal(-1.00000000000000))*(new_r20)*(x229)))+(((IkReal(-1.00000000000000))*(new_r10)*(sj1)*(x230)))); -evalcond[8]=((((sj1)*(x231)))+(((IkReal(-1.00000000000000))*(new_r11)*(sj1)*(x230)))+(((IkReal(-1.00000000000000))*(new_r21)*(x229)))); -evalcond[9]=((IkReal(1.00000000000000))+(((sj1)*(x233)))+(((IkReal(-1.00000000000000))*(new_r22)*(x229)))+(((IkReal(-1.00000000000000))*(new_r12)*(sj1)*(x230)))); -evalcond[10]=((((cj1)*(x232)))+(((new_r20)*(sj1)))+(((IkReal(-1.00000000000000))*(new_r10)*(x226)*(x229)))+(((IkReal(-1.00000000000000))*(x228)))); -evalcond[11]=((((cj1)*(x231)))+(((IkReal(-1.00000000000000))*(sj0)))+(((IkReal(-1.00000000000000))*(new_r11)*(x226)*(x229)))+(((new_r21)*(sj1)))); -if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 ) -{ -continue; -} -} - -{ -std::vector > vinfos(7); -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; -vinfos[6].jointtype = 1; -vinfos[6].foffset = j6; -vinfos[6].indices[0] = _ij6[0]; -vinfos[6].indices[1] = _ij6[1]; -vinfos[6].maxsolutions = _nj6; -std::vector vfree(0); -solutions.AddSolution(vinfos,vfree); -} -} -} - -} - -} -} -} - -} - -} -} -} -} -}}; - - -/// 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 const char* GetKinematicsHash() { return ""; } - -IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } +IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); } #ifdef IKFAST_NAMESPACE } // end namespace #endif - -#endif //MOTOMAN_SIA20D_MANIPULATOR_IKFAST_SOLVER +#endif // MOTOMAN_SIA20D_MANIPULATOR_IKFAST_SOLVER diff --git a/motoman_sia20d_ikfast_manipulator/src/plugin_init.cpp b/motoman_sia20d_ikfast_manipulator/src/plugin_init.cpp index 593fd82d..73e97586 100644 --- a/motoman_sia20d_ikfast_manipulator/src/plugin_init.cpp +++ b/motoman_sia20d_ikfast_manipulator/src/plugin_init.cpp @@ -1,5 +1,7 @@ -//register IKFastKinematicsPlugin as a KinematicsBase implementation +// register IKFastKinematicsPlugin as a KinematicsBase implementation #include #include -PLUGINLIB_EXPORT_CLASS(motoman_sia20d_ikfast_manipulator::ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase); +PLUGINLIB_EXPORT_CLASS( + motoman_sia20d_ikfast_manipulator::ikfast_kinematics_plugin::IKFastKinematicsPlugin, + kinematics::KinematicsBase);