diff --git a/src/bio_ik-kinetic-support/CMakeLists.txt b/src/bio_ik-kinetic-support/CMakeLists.txt deleted file mode 100644 index b46c04f..0000000 --- a/src/bio_ik-kinetic-support/CMakeLists.txt +++ /dev/null @@ -1,129 +0,0 @@ -cmake_minimum_required(VERSION 2.8.12) -project(bio_ik) - -if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") - set(CMAKE_BUILD_TYPE Release) -endif() - -find_package(catkin REQUIRED COMPONENTS - moveit_core - moveit_ros_planning - pluginlib - roscpp - tf - tf_conversions - eigen_conversions -) - -find_package(OpenMP) -# the specific flag is not yet present in cmake 2.8.12 -if(OpenMP_CXX_FOUND OR OPENMP_FOUND) - message(STATUS "OPENMP FOUND") - add_compile_options(${OpenMP_CXX_FLAGS}) - if(NOT OpenMP_CXX_LIBRARIES) - # cmake 2.8.12 does not yet specify the library - assume we might need libgomp - set(OpenMP_LIBS gomp) - else() - set(OpenMP_LIBS ${OpenMP_CXX_LIBRARIES}) - endif() -else() - message(WARNING "OPENMP NOT FOUND. You will suffer performance loss.") - set(OpenMP_LIBS) -endif() - -option(USE_FANN "build the neural-network-based IK solver (experimental)" OFF) -if(USE_FANN) - find_library(FANN_LIBRARIES NAMES fann) - find_path(FANN_INCLUDE_DIRS NAMES fann.h) - if(NOT FANN_INCLUDE_DIRS OR NOT FANN_LIBRARIES) - message(FATAL_ERROR "Neural network solver requested, but libfann was not found.") - else() - message("Found libfann: ${FANN_LIBRARIES} / ${FANN_INCLUDE_DIRS}") - endif() -else() - set(FANN_LIBRARIES) - set(FANN_INCLUDE_DIRS) -endif() - -option(USE_CPPOPTLIB "Include gradient-based solvers from CppNumericalSolvers (bio_ik also provides its own solver)" OFF) -if(USE_CPPOPTLIB) - find_path(CPPOPTLIB_INCLUDE_DIRS - NAMES cppoptlib/solver/bfgssolver.h - HINTS ../../CppNumericalSolvers/include) - if(NOT CPPOPTLIB_INCLUDE_DIRS) - message(FATAL_ERROR "cppoptlib support requested, but the headers could not be found.") - else() - message("Found cppoptlib: ${CPPOPTLIB_INCLUDE_DIRS}") - endif() - add_definitions(-DENABLE_CPP_OPTLIB) -else() - set(CPPOPTLIB_INCLUDE_DIRS) -endif() - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS - moveit_core - moveit_ros_planning - pluginlib - roscpp - tf - tf_conversions -) - -add_compile_options(-frecord-gcc-switches) - -add_compile_options(-std=c++11) - -add_compile_options($<$:-O3>) -add_compile_options($<$:-ftree-vectorize>) -add_compile_options($<$:-ffast-math>) - -include_directories( - include - ${FANN_INCLUDE_DIRS} - ${CPPOPTLIB_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -set(SOURCES - src/goal_types.cpp - src/kinematics_plugin.cpp - src/problem.cpp - - src/ik_test.cpp - src/ik_gradient.cpp - src/ik_evolution_1.cpp - src/ik_evolution_2.cpp -) - -if(USE_FANN) - list(APPEND SOURCES src/ik_neural.cpp) -endif() - -if(USE_CPPOPTLIB) - list(APPEND SOURCES src/ik_cppoptlib.cpp) -endif() - -add_library(${PROJECT_NAME} ${SOURCES}) - -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) - -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${FANN_LIBRARIES} - ${OpenMP_LIBS} - - -static-libgcc - -static-libstdc++ -) - -install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(FILES bio_ik_kinematics_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/bio_ik-kinetic-support/README.md b/src/bio_ik-kinetic-support/README.md deleted file mode 100644 index 480d643..0000000 --- a/src/bio_ik-kinetic-support/README.md +++ /dev/null @@ -1,393 +0,0 @@ -# bio-ik - -## Installation and Setup - -You will need ROS version Indigo or newer (wiki.ros.org). -The software was developed on Ubuntu Linux 16.04 LTS with ROS Kinetic, -but has also been tested on Ubuntu Linux 14.04 LTS with ROS Indigo. -Newer versions of ROS should work, but may need some adaptation. -See below for version specific instructions. - -* Download the `bio_ik` package and unpack into your catkin workspace. -* Run `catkin_make` to compile your workspace: - ``` - roscd - cd src - git clone https://github.com/TAMS-Group/bio_ik.git - roscd - catkin_make - ``` - -* Configure Moveit to use bio-ik as the kinematics solver (see next section). -* Use Moveit to plan and execute motions or use your own code - together with `move_group` node to move your robot. -* As usual, the public API is specified in the public header files for the `bio_ik` package, - located in the `include/bio_ik` subdirectory; - the sources including a few private header files are in the `src` subdirectory. - - -## Basic Usage - -For ease of use and compatibility with existing code, -the bio-ik algorithm is encapsulated as a Moveit kinematics plugin. -Therefore, bio-ik can be used as a direct replacement of -the default Orocos/KDL-based IK solver. -Given the name of an end-effector and a 6-DOF target pose, -bio-ik will search a valid robot joint configuration that reaches the given target. - -In our tests (see below), both in terms of success rate and solution time, -bio-ik regularly outperformed the Orocos [1] solver -and is competitive with trac-ik [2]. -The bio-ik algorithm can also be used for high-DOF system like robot snakes, -and it will automatically converge to the best approximate solutions -for low-DOF arms where some target poses are not reachable exactly. - -While you can write the Moveit configuration files by hand, -the easiest way is to run the Moveit setup assistant for your robot, -and then to select bio-ik as the IK solver when configuring the end effectors. -Once configured, the solver can be called using the standard Moveit API -or used interactively from rviz using the MotionPlanning GUI plugin. - -* Make sure that you have a URDF (or xacro) model for your robot. -* Run the moveit setup assistant to create the Moveit configuration files: - - ``` - rosrun moveit_setup_assistant moveit_setup_assistant - - ``` -* The setup assistant automatically searches for all available IK solver plugins - in your workspace. - Therefore, you can just select select bio-ik as the IK solver - from the drop-down list for every end effector and then configure - the kinematics parameters, namely the default position accuracy (meters) - and the timeout (in seconds). For typical 6-DOF or 7-DOF arms, - an accuracy of 0.001 m (or smaller) and a timeout of 1 msec should be ok. - More complex robots might need a longer timeout. -* Generate the moveit configuration files from the setup assistant. - Of course, you can also edit the `config/kinematics.yaml` configuration - file with your favorite text editor. - For example, a configuration for the PR2 robot might look like this: - - ``` - # example kinematics.yaml for the PR2 robot - right_arm: - # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - # kinematics_solver_attempts: 1 - kinematics_solver: bio_ik/BioIKKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 1 - left_arm: - kinematics_solver: bio_ik/BioIKKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 1 - all: - kinematics_solver: bio_ik/BioIKKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.02 - kinematics_solver_attempts: 1 - - # optional bio-ik configuration parameters - # center_joints_weight: 1 - # minimal_displacement_weight: 1 - # avoid_joint_limits_weight: 1 - ``` - - -* For a first test, run the Moveit-created demo launch. Once rviz is running, - enable the motion planning plugin, then select one of the end effectors - of you robot. Rviz should show an 6-D (position and orientation) - interactive marker for the selected end-effector(s). - Move the interactive marker and watch bio-ik calculating poses for your robot. - - If you also installed the bio-ik demo (see below), you should be able - to run one of the predefined demos: - ``` - roslaunch pr2_bioik_moveit demo.launch - roslaunch pr2_bioik_moveit valve.launch - roslaunch pr2_bioik_moveit dance.launch - ``` - -* You are now ready to use bio-ik from your C/C++ and Python programs, - using the standard Moveit API. - To explicitly request an IK solution in C++: - ``` - robot_model_loader::RobotModelLoader robot_model_loader(robot); - - auto robot_model = robot_model_loader.getModel(); - auto joint_model_group = robot_model->getJointModelGroup(group); - auto tip_names = joint_model_group->getSolverInstance()->getTipFrames(); - - kinematics::KinematicsQueryOptions opts; - opts.return_approximate_solution = true; // optional - - robot_state::RobotState robot_state_ik(robot_model); - - // traditional "basic" bio-ik usage. The end-effector goal poses - // and end-effector link names are passed into the setFromIK() - // call. The KinematicsQueryOptions are empty. - // - bool ok = robot_state_ik.setFromIK( - joint_model_group, // joints to be used for IK - tip_transforms, // multiple end-effector goal poses - tip_names, // names of the end-effector links - attempts, timeout, // solver attempts and timeout - moveit::core::GroupStateValidityCallbackFn(), - opts // mostly empty - ); - ``` - -## Advanced Usage - -For many robot applications, it is essential to specify more than just -a single end-effector pose. Typical examples include - -* two-arm manipulation tasks on two-arm robots (e.g. Baxter) -* multi end-effector tasks with shared kinematic links -* grasping and manipulation tasks with multi-finger hands -* full-body motion on humanoid robots -* reaching tasks with additional constraints (e.g. shoulder position) -* incremental tool motions without robot arm configuration changes -* and many more - -In bio-ik, such tasks are specified as a combination of multiple -individual *goals*. -The algorithm then tries to find a robot configuration -that fulfills all given goals simultaneously by minimizing -a quadratic error function built from the weighted individual goals. -While the current Moveit API does not support multiple-goals tasks directly, -it provides the KinematicQueryOptions class. -Therefore, bio-ik simply provides a set of predefined motion goals, -and a combination of the user-specified goals is passed via Moveit to the IK solver. -No API changes are required in Moveit, but using the IK solver now consists -passing the weighted goals via the KinematicQueryOptions. -The predefined goals include: - -* *PoseGoal*: a full 6-DOF robot pose -* *PositionGoal*: a 3-DOF (x,y,z) position -* *OrientationGoal*: a 3-DOF orientation, encoded as a quaternion (qx,qy,qz,qw) -* *LookAtGoal*: a 3-DOF (x,y,z) position intended as a looking direction - for a camera or robot head -* *JointGoal*: a set of joint angles, e.g. to specify a -* *FunctionGoal*: an arbitrary function of the robot joint values, - e.g. to model underactuated joints or mimic joints -* and several more - - - -To solve a motion problem on your robot, the trick now is to construct -a suitable combination of individual goals. - -![PR2 turning a valve](doc/pr2_vt_0.png) - -In the following example, we want to grasp and then _slowly turn -a valve wheel_ with the left and right gripers of the PR2 robot: - - ``` - bio_ik::BioIKKinematicsQueryOptions ik_options; - ik_options.replace = true; - ik_options.return_approximate_solution = true; - - auto* ll_goal = new bio_ik::PoseGoal(); - auto* lr_goal = new bio_ik::PoseGoal(); - auto* rl_goal = new bio_ik::PoseGoal(); - auto* rr_goal = new bio_ik::PoseGoal(); - ll_goal->setLinkName("l_gripper_l_finger_tip_link"); - lr_goal->setLinkName("l_gripper_r_finger_tip_link"); - rl_goal->setLinkName("r_gripper_l_finger_tip_link"); - rr_goal->setLinkName("r_gripper_r_finger_tip_link"); - ik_options.goals.emplace_back(ll_goal); - ik_options.goals.emplace_back(lr_goal); - ik_options.goals.emplace_back(rl_goal); - ik_options.goals.emplace_back(rr_goal); - ``` - -We also set a couple of secondary goals. -First, we want that the head of the PR2 looks at the center of the valve. -Second, we want to avoid joint-limits on all joints, if possible. -Third, we want that IK solutions are as close as possible to the previous -joint configuration, meaning small and efficient motions. This is handled -by adding the MinimalDisplacementGoal. -Fourth, we want to avoid torso lift motions, which are very slow on the PR2. -All of this is specified easily: - - ``` - auto* lookat_goal = new bio_ik::LookAtGoal(); - lookat_goal->setLinkName("sensor_mount_link"); - ik_options.goals.emplace_back(lookat_goal); - - auto* avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal(); - ik_options.goals.emplace_back(avoid_joint_limits_goal); - - auto* minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal(); - ik_options.goals.emplace_back(minimal_displacement_goal); - - auto* torso_goal = new bio_ik::PositionGoal(); - torso_goal->setLinkName("torso_lift_link"); - torso_goal->setWeight(1); - torso_goal->setPosition(tf::Vector3( -0.05, 0, 1.0 )); - ik_options.goals.emplace_back(torso_goal); - ``` - -For the actual turning motion, we calculate a set of required gripper -poses in a loop: - ``` - for(int i = 0; ; i++) { - tf::Vector3 center(0.7, 0, 1); - - double t = i * 0.1; - double r = 0.1; - double a = sin(t) * 1; - double dx = fmin(0.0, cos(t) * -0.1); - double dy = cos(a) * r; - double dz = sin(a) * r; - - tf::Vector3 dl(dx, +dy, +dz); - tf::Vector3 dr(dx, -dy, -dz); - tf::Vector3 dg = tf::Vector3(0, cos(a), sin(a)) * (0.025 + fmin(0.025, fmax(0.0, cos(t) * 0.1))); - - ll_goal->setPosition(center + dl + dg); - lr_goal->setPosition(center + dl - dg); - rl_goal->setPosition(center + dr + dg); - rr_goal->setPosition(center + dr - dg); - - double ro = 0; - ll_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - lr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - rl_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - rr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro)); - - lookat_goal->setAxis(tf::Vector3(1, 0, 0)); - lookat_goal->setTarget(rr_goal->getPosition()); - - // "advanced" bio-ik usage. The call parameters for the end-effector - // poses and end-effector link names are left empty; instead the - // requested goals and weights are passed via the ik_options object. - // - robot_state.setFromIK( - joint_model_group, // active PR2 joints - EigenSTL::vector_Affine3d(), // no explicit poses here - std::vector(), // no end effector links here - 0, 0.0, // take values from YAML file - moveit::core::GroupStateValidityCallbackFn(), - ik_options // four gripper goals and secondary goals - ); - - ... // check solution validity and actually move the robot - } - ``` - -When you execute the code, the PR2 will reach for the valve wheel -and turn it. Every once in a while it can't reach the valve with -its current arm configuration and will regrasp the wheel. - - -## How it works - -The bio-ik solver is based on a memetic algorithm that combines -traditional gradient-based search with a hybrid genetic -and particle-swarm optimization [3]. -See [4] for the basic idea and the details of the evolutionary operators -and [5] for the description of the algorithm applied to many IK and manipulation tasks. - -Internally, vectors of all robot joint values are used to encode -different intermediate solutions (the *genotype* of the genetic algorithm). -During the optimization, joint values are always checked against the -active lower and upper joint limits, so that only valid robot configurations -are generated. - -To calculate the fitness of individuals, the cumulative error over all -given individual goals is calculated. -Any individual with zero error is an exact solution for the IK problem, -while individuals with small error correspond to approximate solutions. - -Individuals are sorted by their fitness, and gradient-based optimization -is tried on the best few configuration, resulting in fast convergence -and good performance for many problems. -If no solution is found from the gradient-based optimization, -new individuals are created by a set of mutation and recombination operators, -resulting in good search-space exploration. - - - -## Example Performance data - -To be written. - -## Running the Self-Tests - -We have tested bio-ik on many different robot arms, -both using the tranditional single end-effector API -and the advanced multi end-effector API based on the KinematicsQueryOptions. - -One simple selftest consists of generating random valid robot configurations, -running forward kinematics to calculate the resulting end-effector pose, -and the querying the IK plugin to find a suitable robot joint configuration. -Success is then checked by running forrward kinematics again and checking -that the end-effector pose for the generated IK solution matches the target pose. -This approach can be run easily for thousands or millions of random poses, -samples the full workspace of the robot, -and allows to quickly generate success-rate and solution-time estimates -for the selected IK solver. - -Of course, running the tests requires installing the corresponding robot -models and adds a lot of dependencies. -Therefore, those tests are not included in the standard bio-ik package, -but are packaged separately. - -For convenience, we provide the `pr2_bioik_moveit` package, -which also includes a few bio-ik demos for the PR2 service robot. -These are kinematics only demos; but of course you can also try -running the demos on the real robot (if you have one) or the Gazebo -simulator (if you installed Gazebo). - -Simply clone the PR2 description package (inside `pr2_common`) -and the `pr2_bioik_moveit` package into your catkin workspace: - ``` - roscd - cd src - git clone https://github.com/PR2/pr2_common.git - git clone https://github.com/TAMS/pr2_bioik_moveit.git - catkin_make - ``` - -For the FK-IK-FK performance test, please run - - ``` - roslaunch pr2_bioik_moveit env_pr2.launch - roslaunch pr2_bioik_moveit test_fk_ik.launch - ... // wait for test completion and results summary - ``` - - - - - -## References - - 1. Orocos Kinematics and Dynamics, http://www.orocos.org - - 2. P. Beeson and B. Ames, *TRAC-IK: - An open-source library for improved solving of generic inverse kinematics*, - Proceedings of the IEEE RAS Humanoids Conference, Seoul, Korea, November 2015. - - 3. Philipp Ruppel, Prformance optimization and implementation - of evolutionary inverse kinematics in ROS*, - MSc thesis, University of Hamburg, 2017 - [PDF](https://tams.informatik.uni-hamburg.de/publications/2017/MSc_Philipp_Ruppel.pdf) - - - 4. Sebastian Starke, Norman Hendrich, Jianwei Zhang, *A Memetic - Evolutionary Algorithm for Real-Time Articulated Kinematic Motion*, - IEEE Intl. Congress on Evolutionary Computation (CEC-2017), p.2437-2479, June 4-8, 2017, - San Sebastian, Spain. - DOI: [10.1109/CEC.2017.7969605](http://doi.org/10.1109/CEC.2017.7969605) - - 5. Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, *Multi-Objective - Evolutionary Optimisation for Inverse Kinematics - on Highly Articulated and Humanoid Robots*, - IEEE Intl. Conference on Intelligent Robots and Systems (IROS-2017), - September 24-28, 2017, Vancouver, Canada - - diff --git a/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml b/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml deleted file mode 100644 index cf68db3..0000000 --- a/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/src/bio_ik-kinetic-support/doc/goals.pdf b/src/bio_ik-kinetic-support/doc/goals.pdf deleted file mode 100644 index 53fe575..0000000 Binary files a/src/bio_ik-kinetic-support/doc/goals.pdf and /dev/null differ diff --git a/src/bio_ik-kinetic-support/doc/pr2_vt_0.png b/src/bio_ik-kinetic-support/doc/pr2_vt_0.png deleted file mode 100644 index f311fdf..0000000 Binary files a/src/bio_ik-kinetic-support/doc/pr2_vt_0.png and /dev/null differ diff --git a/src/bio_ik-kinetic-support/doc/solvers.pdf b/src/bio_ik-kinetic-support/doc/solvers.pdf deleted file mode 100644 index 80f2c65..0000000 Binary files a/src/bio_ik-kinetic-support/doc/solvers.pdf and /dev/null differ diff --git a/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h b/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h deleted file mode 100644 index 6303fd2..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h +++ /dev/null @@ -1,50 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#include - -#include "goal.h" -#include "goal_types.h" - -namespace bio_ik -{ - -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/frame.h b/src/bio_ik-kinetic-support/include/bio_ik/frame.h deleted file mode 100644 index eadafc4..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/frame.h +++ /dev/null @@ -1,258 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include - -#include -#include - -namespace bio_ik -{ - -typedef tf::Quaternion Quaternion; -typedef tf::Vector3 Vector3; - -struct Frame -{ - Vector3 pos; - double __padding[4 - (sizeof(Vector3) / sizeof(double))]; - Quaternion rot; - inline Frame() {} - inline Frame(const tf::Vector3& pos, const tf::Quaternion& rot) - : pos(pos) - , rot(rot) - { - } - explicit inline Frame(const KDL::Frame& kdl) - { - pos = tf::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z()); - double qx, qy, qz, qw; - kdl.M.GetQuaternion(qx, qy, qz, qw); - rot = tf::Quaternion(qx, qy, qz, qw); - } - explicit inline Frame(const geometry_msgs::Pose& msg) - { - tf::quaternionMsgToTF(msg.orientation, rot); - pos = tf::Vector3(msg.position.x, msg.position.y, msg.position.z); - } - explicit inline Frame(const Eigen::Affine3d& f) - { - pos = tf::Vector3(f.translation().x(), f.translation().y(), f.translation().z()); - Eigen::Quaterniond q(f.rotation()); - rot = tf::Quaternion(q.x(), q.y(), q.z(), q.w()); - } - - inline const Vector3& getPosition() const { return pos; } - inline const Quaternion& getOrientation() const { return rot; } - inline void setPosition(const Vector3& p) { pos = p; } - inline void setOrientation(const Quaternion& q) { rot = q; } - -private: - template struct IdentityFrameTemplate - { - static const Frame identity_frame; - }; - -public: - static inline const Frame& identity() { return IdentityFrameTemplate<0>::identity_frame; } -}; - -inline void frameToKDL(const Frame& frame, KDL::Frame& kdl_frame) -{ - KDL::Rotation kdl_rot; - KDL::Vector kdl_pos; - tf::quaternionTFToKDL(frame.rot, kdl_rot); - tf::vectorTFToKDL(frame.pos, kdl_pos); - kdl_frame = KDL::Frame(kdl_rot, kdl_pos); -} - -template const Frame Frame::IdentityFrameTemplate::identity_frame(Vector3(0, 0, 0), Quaternion(0, 0, 0, 1)); - -static std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; } - -__attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q, const tf::Vector3& v, tf::Vector3& r) -{ - double v_x = v.x(); - double v_y = v.y(); - double v_z = v.z(); - - // if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf::Vector3(0, 0, 0); return; } - // if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf::Vector3(0, 0, 0); return; } - - double q_x = q.x(); - double q_y = q.y(); - double q_z = q.z(); - double q_w = q.w(); - - if((v_x == 0 && v_y == 0 && v_z == 0) || (q_x == 0 && q_y == 0 && q_z == 0 && q_w == 1)) - { - r = v; - return; - } - // if((v_x + v_y + v_z == 0 && v_x == 0 && v_y == 0) || (q_x + q_y + q_z == 0 && q_x == 0 && q_y == 0 && q_w == 1)) { r = v; return; } - // if(q_x == 0 && q_y == 0 && q_z == 0 && q_w == 1) { r = v; return; } - - double t_x = q_y * v_z - q_z * v_y; - double t_y = q_z * v_x - q_x * v_z; - double t_z = q_x * v_y - q_y * v_x; - - double r_x = q_w * t_x + q_y * t_z - q_z * t_y; - double r_y = q_w * t_y + q_z * t_x - q_x * t_z; - double r_z = q_w * t_z + q_x * t_y - q_y * t_x; - - r_x += r_x; - r_y += r_y; - r_z += r_z; - - r_x += v_x; - r_y += v_y; - r_z += v_z; - - r.setX(r_x); - r.setY(r_y); - r.setZ(r_z); -} - -__attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p, const tf::Quaternion& q, tf::Quaternion& r) -{ - double p_x = p.x(); - double p_y = p.y(); - double p_z = p.z(); - double p_w = p.w(); - - double q_x = q.x(); - double q_y = q.y(); - double q_z = q.z(); - double q_w = q.w(); - - double r_x = (p_w * q_x + p_x * q_w) + (p_y * q_z - p_z * q_y); - double r_y = (p_w * q_y - p_x * q_z) + (p_y * q_w + p_z * q_x); - double r_z = (p_w * q_z + p_x * q_y) - (p_y * q_x - p_z * q_w); - double r_w = (p_w * q_w - p_x * q_x) - (p_y * q_y + p_z * q_z); - - r.setX(r_x); - r.setY(r_y); - r.setZ(r_z); - r.setW(r_w); -} - -__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, Frame& r) -{ - tf::Vector3 d; - quat_mul_vec(a.rot, b.pos, d); - r.pos = a.pos + d; - quat_mul_quat(a.rot, b.rot, r.rot); -} - -__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, const Frame& c, Frame& r) -{ - Frame tmp; - concat(a, b, tmp); - concat(tmp, c, r); -} - -__attribute__((always_inline)) inline void quat_inv(const tf::Quaternion& q, tf::Quaternion& r) -{ - r.setX(-q.x()); - r.setY(-q.y()); - r.setZ(-q.z()); - r.setW(q.w()); -} - -__attribute__((always_inline)) inline void invert(const Frame& a, Frame& r) -{ - Frame tmp; - quat_inv(a.rot, r.rot); - quat_mul_vec(r.rot, -a.pos, r.pos); -} - -__attribute__((always_inline)) inline void change(const Frame& a, const Frame& b, const Frame& c, Frame& r) -{ - Frame tmp; - invert(b, tmp); - concat(a, tmp, c, r); -} - -__attribute__((always_inline)) inline Frame inverse(const Frame& f) -{ - Frame r; - invert(f, r); - return r; -} - -__attribute__((always_inline)) inline Frame operator*(const Frame& a, const Frame& b) -{ - Frame r; - concat(a, b, r); - return r; -} - -__attribute__((always_inline)) inline Frame& operator*=(Frame& a, const Frame& b) -{ - a = a * b; - return a; -} - -__attribute__((always_inline)) inline void normalizeFast(Quaternion& q) -{ - double f = (3.0 - q.length2()) * 0.5; - q.setX(q.x() * f); - q.setY(q.y() * f); - q.setZ(q.z() * f); - q.setW(q.w() * f); -} - -__attribute__((always_inline)) inline KDL::Twist frameTwist(const Frame& a, const Frame& b) -{ - auto frame = inverse(a) * b; - KDL::Twist t; - t.vel.x(frame.pos.x()); - t.vel.y(frame.pos.y()); - t.vel.z(frame.pos.z()); - - double ra = frame.rot.getAngle(); - // double ra = frame.rot.getAngleShortestPath(); - if(ra > +M_PI) ra -= 2 * M_PI; - // if(ra < -M_PI) ra += 2 * M_PI; - - auto r = frame.rot.getAxis() * ra; - t.rot.x(r.x()); - t.rot.y(r.y()); - t.rot.z(r.z()); - - return t; -} -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/goal.h b/src/bio_ik-kinetic-support/include/bio_ik/goal.h deleted file mode 100644 index 480ac6d..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/goal.h +++ /dev/null @@ -1,130 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include "frame.h" - -#include - -#include -#include - -namespace bio_ik -{ - -class RobotInfo; - -class GoalContext -{ -protected: - const double* active_variable_positions_; - const Frame* tip_link_frames_; - std::vector goal_variable_indices_; - std::vector goal_link_indices_; - bool goal_secondary_; - std::vector goal_link_names_, goal_variable_names_; - double goal_weight_; - const moveit::core::JointModelGroup* joint_model_group_; - std::vector problem_active_variables_; - std::vector problem_tip_link_indices_; - std::vector initial_guess_; - std::vector velocity_weights_; - const RobotInfo* robot_info_; - mutable std::vector temp_vector_; - -public: - GoalContext() {} - inline const Frame& getLinkFrame(size_t i = 0) const { return tip_link_frames_[goal_link_indices_[i]]; } - inline const double getVariablePosition(size_t i = 0) const - { - auto j = goal_variable_indices_[i]; - if(j >= 0) - return active_variable_positions_[j]; - else - return initial_guess_[-1 - j]; - } - inline const Frame& getProblemLinkFrame(size_t i) const { return tip_link_frames_[i]; } - inline size_t getProblemLinkCount() const { return problem_tip_link_indices_.size(); } - inline size_t getProblemLinkIndex(size_t i) const { return problem_tip_link_indices_[i]; } - inline double getProblemVariablePosition(size_t i) const { return active_variable_positions_[i]; } - inline size_t getProblemVariableCount() const { return problem_active_variables_.size(); } - inline size_t getProblemVariableIndex(size_t i) const { return problem_active_variables_[i]; } - inline double getProblemVariableInitialGuess(size_t i) const { return initial_guess_[problem_active_variables_[i]]; } - inline double getProblemVariableWeight(size_t i) const { return velocity_weights_[i]; } - inline const RobotInfo& getRobotInfo() const { return *robot_info_; } - void addLink(const std::string& name) { goal_link_names_.push_back(name); } - void addVariable(const std::string& name) { goal_variable_names_.push_back(name); } - void setSecondary(bool secondary) { goal_secondary_ = secondary; } - void setWeight(double weight) { goal_weight_ = weight; } - const moveit::core::JointModelGroup& getJointModelGroup() const { return *joint_model_group_; } - const moveit::core::RobotModel& getRobotModel() const { return joint_model_group_->getParentModel(); } - std::vector& getTempVector() const { return temp_vector_; } - friend class Problem; -}; - -class Goal -{ -protected: - bool secondary_; - double weight_; - -public: - Goal() - : weight_(1) - , secondary_(false) - { - } - virtual ~Goal() {} - bool isSecondary() const { return secondary_; } - double getWeight() const { return weight_; } - void setWeight(double w) { weight_ = w; } - virtual void describe(GoalContext& context) const - { - context.setSecondary(secondary_); - context.setWeight(weight_); - } - virtual double evaluate(const GoalContext& context) const { return 0; } -}; - -struct BioIKKinematicsQueryOptions : kinematics::KinematicsQueryOptions -{ - std::vector> goals; - std::vector fixed_joints; - bool replace; - mutable double solution_fitness; - BioIKKinematicsQueryOptions(); - ~BioIKKinematicsQueryOptions(); -}; -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h b/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h deleted file mode 100644 index a6957a0..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h +++ /dev/null @@ -1,661 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include "goal.h" - -#include "robot_info.h" - -#include - -#include -#include - -#include -#include - -#include -#include - -namespace bio_ik -{ - -class LinkGoalBase : public Goal -{ - std::string link_name_; - -public: - LinkGoalBase() - { - weight_ = 1; - link_name_ = ""; - } - LinkGoalBase(const std::string& link_name, double weight) - { - weight_ = weight; - link_name_ = link_name; - } - virtual void describe(GoalContext& context) const - { - Goal::describe(context); - context.addLink(link_name_); - } - void setLinkName(const std::string& link_name) { link_name_ = link_name; } - const std::string& getLinkName() const { return link_name_; } -}; - -class PositionGoal : public LinkGoalBase -{ - tf::Vector3 position_; - -public: - PositionGoal() - : position_(0, 0, 0) - { - } - PositionGoal(const std::string& link_name, const tf::Vector3& position, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position_(position) - { - } - inline const tf::Vector3& getPosition() const { return position_; } - inline void setPosition(const tf::Vector3& position) { position_ = position; } - virtual double evaluate(const GoalContext& context) const { return context.getLinkFrame().getPosition().distance2(getPosition()); } -}; - -class OrientationGoal : public LinkGoalBase -{ - tf::Quaternion orientation_; - -public: - OrientationGoal() - : orientation_(0, 0, 0, 1) - { - } - OrientationGoal(const std::string& link_name, const tf::Quaternion& orientation, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , orientation_(orientation.normalized()) - { - } - inline const tf::Quaternion& getOrientation() const { return orientation_; } - inline void setOrientation(const tf::Quaternion& orientation) { orientation_ = orientation.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - // return getOrientation().distance2(context.getLinkFrame().getOrientation()); - // return (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2(); - return fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2()); - /*return - (getOrientation() - context.getLinkFrame().getOrientation()).length2() * - (getOrientation() + context.getLinkFrame().getOrientation()).length2() * 0.5;*/ - } -}; - -class PoseGoal : public LinkGoalBase -{ - Frame frame_; - double rotation_scale_; - -public: - PoseGoal() - : rotation_scale_(0.5) - , frame_(Frame::identity()) - { - } - PoseGoal(const std::string& link_name, const tf::Vector3& position, const tf::Quaternion& orientation, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , frame_(position, orientation.normalized()) - , rotation_scale_(0.5) - { - } - inline const tf::Vector3& getPosition() const { return frame_.getPosition(); } - inline void setPosition(const tf::Vector3& position) { frame_.setPosition(position); } - inline const tf::Quaternion& getOrientation() const { return frame_.getOrientation(); } - inline void setOrientation(const tf::Quaternion& orientation) { frame_.setOrientation(orientation.normalized()); } - inline double getRotationScale() const { return rotation_scale_; } - inline void setRotationScale(double rotation_scale) { rotation_scale_ = rotation_scale; } - virtual double evaluate(const GoalContext& context) const - { - double e = 0.0; - e += context.getLinkFrame().getPosition().distance2(getPosition()); - - /*e += - (getOrientation() - context.getLinkFrame().getOrientation()).length2() * - (getOrientation() + context.getLinkFrame().getOrientation()).length2() * - (rotation_scale_ * rotation_scale_) * 0.5;*/ - - /*double a = getOrientation().angleShortestPath(context.getLinkFrame().getOrientation()); - e += a * a; - return e;*/ - - /*e += 1 - getOrientation().dot(context.getLinkFrame().getOrientation()); - return e;*/ - - /*double l = getOrientation().length2() * context.getLinkFrame().getOrientation().length2(); - //double x = _mm_rsqrt_ss(_mm_set_ss((float)l))[0]; - double x = 1.0 / l; - e += (1 - getOrientation().dot(context.getLinkFrame().getOrientation()) * x) * (rotation_scale_ * rotation_scale_); - return e;*/ - - e += fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2()) * (rotation_scale_ * rotation_scale_); - - // e += (1.0 - getOrientation().dot(context.getLinkFrame().getOrientation())) * (rotation_scale_ * rotation_scale_); - - // e += (getOrientation() - context.getLinkFrame().getOrientation()).length2() * (rotation_scale_ * rotation_scale_); - // ROS_ERROR("r %f", (getOrientation() - context.getLinkFrame().getOrientation()).length2()); - // e += (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2() * (rotation_scale_ * rotation_scale_); - return e; - } -}; - -class LookAtGoal : public LinkGoalBase -{ - tf::Vector3 axis_; - tf::Vector3 target_; - -public: - LookAtGoal() - : axis_(1, 0, 0) - , target_(0, 0, 0) - { - } - LookAtGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& target, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , axis_(axis) - , target_(target) - { - } - const tf::Vector3& getAxis() const { return axis_; } - const tf::Vector3& getTarget() const { return target_; } - void setAxis(const tf::Vector3& axis) { axis_ = axis.normalized(); } - void setTarget(const tf::Vector3& target) { target_ = target; } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - tf::Vector3 axis; - quat_mul_vec(fb.getOrientation(), axis_, axis); - return (target_ - fb.getPosition()).normalized().distance2(axis.normalized()); - // return (target_ - axis * axis.dot(target_ - fb.getPosition())).distance2(fb.getPosition()); - } -}; - -class MaxDistanceGoal : public LinkGoalBase -{ - tf::Vector3 target; - double distance; - -public: - MaxDistanceGoal() - : target(0, 0, 0) - , distance(1) - { - } - MaxDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , target(target) - , distance(distance) - { - } - const tf::Vector3& getTarget() const { return target; } - void setTarget(const tf::Vector3& t) { target = t; } - double getDistance() const { return distance; } - void setDistance(double d) { distance = d; } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - double d = fmax(0.0, fb.getPosition().distance(target) - distance); - return d * d; - } -}; - -class MinDistanceGoal : public LinkGoalBase -{ - tf::Vector3 target; - double distance; - -public: - MinDistanceGoal() - : target(0, 0, 0) - , distance(1) - { - } - MinDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , target(target) - , distance(distance) - { - } - const tf::Vector3& getTarget() const { return target; } - void setTarget(const tf::Vector3& t) { target = t; } - double getDistance() const { return distance; } - void setDistance(double d) { distance = d; } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - double d = fmax(0.0, distance - fb.getPosition().distance(target)); - return d * d; - } -}; - -class LineGoal : public LinkGoalBase -{ - tf::Vector3 position; - tf::Vector3 direction; - -public: - LineGoal() - : position(0, 0, 0) - , direction(0, 0, 0) - { - } - LineGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& direction, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , direction(direction.normalized()) - { - } - const tf::Vector3& getPosition() const { return position; } - void setPosition(const tf::Vector3& p) { position = p; } - const tf::Vector3& getDirection() const { return direction; } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - return position.distance2(fb.getPosition() - direction * direction.dot(fb.getPosition() - position)); - } -}; - -class TouchGoal : public LinkGoalBase -{ - tf::Vector3 position; - tf::Vector3 normal; - struct CollisionShape - { - std::vector vertices; - std::vector points; - std::vector polygons; - std::vector plane_normals; - std::vector plane_dis; - collision_detection::FCLGeometryConstPtr geometry; - Frame frame; - std::vector> edges; - }; - struct CollisionLink - { - bool initialized; - std::vector> shapes; - CollisionLink() - : initialized(false) - { - } - }; - struct CollisionModel - { - std::vector collision_links; - }; - mutable CollisionModel* collision_model; - mutable const moveit::core::LinkModel* link_model; - -public: - TouchGoal() - : position(0, 0, 0) - , normal(0, 0, 0) - { - } - TouchGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& normal, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , normal(normal.normalized()) - { - } - virtual void describe(GoalContext& context) const; - virtual double evaluate(const GoalContext& context) const; -}; - -class AvoidJointLimitsGoal : public Goal -{ -public: - AvoidJointLimitsGoal(double weight = 1.0, bool secondary = true) - { - weight_ = weight; - secondary_ = secondary; - } - virtual double evaluate(const GoalContext& context) const - { - auto& info = context.getRobotInfo(); - double sum = 0.0; - for(size_t i = 0; i < context.getProblemVariableCount(); i++) - { - size_t ivar = context.getProblemVariableIndex(i); - if(info.getClipMax(ivar) == DBL_MAX) continue; - double d = context.getProblemVariablePosition(i) - (info.getMin(ivar) + info.getMax(ivar)) * 0.5; - d = fmax(0.0, fabs(d) * 2.0 - info.getSpan(ivar) * 0.5); - d *= context.getProblemVariableWeight(i); - sum += d * d; - } - return sum; - } -}; - -class CenterJointsGoal : public Goal -{ -public: - CenterJointsGoal(double weight = 1.0, bool secondary = true) - { - weight_ = weight; - secondary_ = secondary; - } - virtual double evaluate(const GoalContext& context) const - { - auto& info = context.getRobotInfo(); - double sum = 0.0; - for(size_t i = 0; i < context.getProblemVariableCount(); i++) - { - size_t ivar = context.getProblemVariableIndex(i); - if(info.getClipMax(ivar) == DBL_MAX) continue; - double d = context.getProblemVariablePosition(i) - (info.getMin(ivar) + info.getMax(ivar)) * 0.5; - d *= context.getProblemVariableWeight(i); - sum += d * d; - } - return sum; - } -}; - -class MinimalDisplacementGoal : public Goal -{ -public: - MinimalDisplacementGoal(double weight = 1.0, bool secondary = true) - { - weight_ = weight; - secondary_ = secondary; - } - virtual double evaluate(const GoalContext& context) const - { - double sum = 0.0; - for(size_t i = 0; i < context.getProblemVariableCount(); i++) - { - double d = context.getProblemVariablePosition(i) - context.getProblemVariableInitialGuess(i); - d *= context.getProblemVariableWeight(i); - sum += d * d; - } - return sum; - } -}; - -class JointVariableGoal : public Goal -{ - std::string variable_name; - double variable_position; - -public: - JointVariableGoal() - : variable_position(0) - { - } - JointVariableGoal(const std::string& variable_name, double variable_position, double weight = 1.0, bool secondary = false) - : variable_name(variable_name) - , variable_position(variable_position) - { - weight_ = weight; - secondary_ = secondary; - } - double getVariablePosition() const { return variable_position; } - void setVariablePosition(double p) { variable_position = p; } - const std::string& getVariableName() const { return variable_name; } - void setVariableName(const std::string& n) { variable_name = n; } - virtual void describe(GoalContext& context) const - { - Goal::describe(context); - context.addVariable(variable_name); - } - virtual double evaluate(const GoalContext& context) const - { - double d = variable_position - context.getVariablePosition(); - return d * d; - } -}; - -class JointFunctionGoal : public Goal -{ - std::vector variable_names; - std::function&)> function; - -public: - JointFunctionGoal() {} - JointFunctionGoal(const std::vector& variable_names, const std::function&)>& function, double weight = 1.0, bool secondary = false) - : variable_names(variable_names) - , function(function) - { - weight_ = weight; - secondary_ = secondary; - } - void setJointVariableNames(const std::vector& n) { variable_names = n; } - void setJointVariableFunction(const std::function&)>& f) { function = f; } - virtual void describe(GoalContext& context) const - { - Goal::describe(context); - for(auto& variable_name : variable_names) - context.addVariable(variable_name); - } - virtual double evaluate(const GoalContext& context) const - { - auto& temp_vector = context.getTempVector(); - temp_vector.resize(variable_names.size()); - for(size_t i = 0; i < variable_names.size(); i++) - temp_vector[i] = context.getVariablePosition(i); - function(temp_vector); - double sum = 0.0; - for(size_t i = 0; i < variable_names.size(); i++) - { - double d = temp_vector[i] - context.getVariablePosition(i); - sum += d * d; - } - return sum; - } -}; - -class BalanceGoal : public Goal -{ - tf::Vector3 target_, axis_; - struct BalanceInfo - { - tf::Vector3 center; - double weight; - }; - mutable std::vector balance_infos; - -public: - BalanceGoal() - : target_(0, 0, 0) - , axis_(0, 0, 1) - { - } - BalanceGoal(const tf::Vector3& target, double weight = 1.0) - : target_(target) - , axis_(0, 0, 1) - { - weight_ = weight; - } - const tf::Vector3& getTarget() const { return target_; } - const tf::Vector3& getAxis() const { return axis_; } - void setTarget(const tf::Vector3& target) { target_ = target; } - void setAxis(const tf::Vector3& axis) { axis_ = axis; } - virtual void describe(GoalContext& context) const; - virtual double evaluate(const GoalContext& context) const; -}; - -class LinkFunctionGoal : public LinkGoalBase -{ - std::function function; - -public: - LinkFunctionGoal() {} - LinkFunctionGoal(const std::string& link_name, const std::function& function, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , function(function) - { - } - void setLinkFunction(const std::function& f) { function = f; } - virtual double evaluate(const GoalContext& context) const { return function(context.getLinkFrame().getPosition(), context.getLinkFrame().getOrientation()); } -}; - -class SideGoal : public LinkGoalBase -{ - tf::Vector3 axis; - tf::Vector3 direction; - -public: - SideGoal() - : axis(0, 0, 1) - , direction(0, 0, 1) - { - } - SideGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , axis(axis) - , direction(direction) - { - } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - double f = fmax(0.0, v.dot(direction)); - return f * f; - } -}; - -class DirectionGoal : public LinkGoalBase -{ - tf::Vector3 axis; - tf::Vector3 direction; - -public: - DirectionGoal() - : axis(0, 0, 1) - , direction(0, 0, 1) - { - } - DirectionGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , axis(axis) - , direction(direction) - { - } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - virtual double evaluate(const GoalContext& context) const - { - auto& fb = context.getLinkFrame(); - Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - return v.distance2(direction); - } -}; - -class ConeGoal : public LinkGoalBase -{ - tf::Vector3 position; - double position_weight; - tf::Vector3 axis; - tf::Vector3 direction; - double angle; - -public: - const tf::Vector3& getPosition() const { return position; } - double getPositionWeight() const { return position_weight; } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - double getAngle() const { return angle; } - void setPosition(const tf::Vector3& p) { position = p; } - void setPositionWeight(double w) { position_weight = w; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } - void setAngle(double a) { angle = a; } - ConeGoal() - : position(0, 0, 0) - , position_weight(0) - , axis(0, 0, 1) - , direction(0, 0, 1) - , angle(0) - { - } - ConeGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(0, 0, 0) - , position_weight(0) - , axis(axis) - , direction(direction) - , angle(angle) - { - } - ConeGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , position_weight(1) - , axis(axis) - , direction(direction) - , angle(angle) - { - } - ConeGoal(const std::string& link_name, const tf::Vector3& position, double position_weight, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) - : LinkGoalBase(link_name, weight) - , position(position) - , position_weight(position_weight) - , axis(axis) - , direction(direction) - , angle(angle) - { - } - virtual double evaluate(const GoalContext& context) const - { - double sum = 0.0; - auto& fb = context.getLinkFrame(); - Vector3 v; - quat_mul_vec(fb.getOrientation(), axis, v); - double d = fmax(0.0, v.angle(direction) - angle); - sum += d * d; - double w = position_weight; - sum += w * w * (position - fb.getPosition()).length2(); - return sum; - } -}; -} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h b/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h deleted file mode 100644 index 70e6676..0000000 --- a/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h +++ /dev/null @@ -1,126 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include -#include -#include - -#include - -namespace bio_ik -{ - -class RobotInfo -{ - struct VariableInfo - { - double clip_min, clip_max; - double span; - double min; - double max; - double max_velocity, max_velocity_rcp; - }; - std::vector variables; - std::vector activeVariables; - std::vector variable_joint_types; - moveit::core::RobotModelConstPtr robot_model; - - __attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi) - { - if(__builtin_expect(v < lo, 0)) v = lo; - if(__builtin_expect(v > hi, 0)) v = hi; - return v; - } - -public: - RobotInfo() {} - RobotInfo(moveit::core::RobotModelConstPtr model) - : robot_model(model) - { - for(auto& name : model->getVariableNames()) - { - auto& bounds = model->getVariableBounds(name); - - VariableInfo info; - - bool bounded = bounds.position_bounded_; - - auto* joint_model = model->getJointOfVariable(variables.size()); - if(auto* revolute = dynamic_cast(joint_model)) - - if(bounds.max_position_ - bounds.min_position_ >= 2 * M_PI * 0.9999) bounded = false; - - info.min = bounds.min_position_; - info.max = bounds.max_position_; - - info.clip_min = bounded ? info.min : -DBL_MAX; - info.clip_max = bounded ? info.max : +DBL_MAX; - - info.span = info.max - info.min; - - if(!(info.span >= 0 && info.span < FLT_MAX)) info.span = 1; - - info.max_velocity = bounds.max_velocity_; - info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0; - - variables.push_back(info); - } - - for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++) - { - variable_joint_types.push_back(model->getJointOfVariable(ivar)->getType()); - } - } - -public: - __attribute__((always_inline)) inline double clip(double p, size_t i) const - { - auto& info = variables[i]; - return clamp2(p, info.clip_min, info.clip_max); - } - - inline double getSpan(size_t i) const { return variables[i].span; } - inline double getClipMin(size_t i) const { return variables[i].clip_min; } - inline double getClipMax(size_t i) const { return variables[i].clip_max; } - inline double getMin(size_t i) const { return variables[i].min; } - inline double getMax(size_t i) const { return variables[i].max; } - - inline bool isRevolute(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::REVOLUTE; } - inline bool isPrismatic(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::PRISMATIC; } - inline double getMaxVelocity(size_t i) const { return variables[i].max_velocity; } - inline double getMaxVelocityRcp(size_t i) const { return variables[i].max_velocity_rcp; } -}; -} diff --git a/src/bio_ik-kinetic-support/package.xml b/src/bio_ik-kinetic-support/package.xml deleted file mode 100644 index 719ca77..0000000 --- a/src/bio_ik-kinetic-support/package.xml +++ /dev/null @@ -1,47 +0,0 @@ - - - - bio_ik - 1.0.0 - BioIK for ROS - BSD - - Philipp Sebastian Ruppel - - Philipp Sebastian Ruppel - Michael Goerner - - - catkin - - moveit_core - moveit_core - - pluginlib - pluginlib - - eigen - eigen - - moveit_ros_planning - moveit_ros_planning - - roscpp - roscpp - - tf - tf - - tf_conversions - tf_conversions - - eigen_conversions - eigen_conversions - - rosunit - - - - - - diff --git a/src/bio_ik-kinetic-support/src/forward_kinematics.h b/src/bio_ik-kinetic-support/src/forward_kinematics.h deleted file mode 100644 index 904a1f7..0000000 --- a/src/bio_ik-kinetic-support/src/forward_kinematics.h +++ /dev/null @@ -1,1503 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include "utils.h" -#include -#include -#include -#include - -#if defined(__x86_64__) || defined(__i386__) - -#include -#include -#include - -#if(__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 9)) -#define FUNCTION_MULTIVERSIONING 1 -#else -#define FUNCTION_MULTIVERSIONING 0 -#endif - -#else - -#define FUNCTION_MULTIVERSIONING 0 - -#endif - -namespace bio_ik -{ - -// computes and caches local joint frames -class RobotJointEvaluator -{ -private: - std::vector joint_cache_variables; - std::vector joint_cache_frames; - std::vector link_frames; - -protected: - std::vector joint_axis_list; - moveit::core::RobotModelConstPtr robot_model; - std::vector variables; - -public: - inline void getJointFrame(const moveit::core::JointModel* joint_model, const double* variables, Frame& frame) - { - auto joint_type = joint_model->getType(); - if(joint_type == moveit::core::JointModel::FIXED) - { - frame = Frame::identity(); - return; - } - size_t joint_index = joint_model->getJointIndex(); - switch(joint_type) - { - case moveit::core::JointModel::REVOLUTE: - { - auto axis = joint_axis_list[joint_index]; - - auto v = variables[joint_model->getFirstVariableIndex()]; - - /*v *= 1.0 / (2 * M_PI); - v -= std::floor(v); - v *= (2 * M_PI);*/ - - auto half_angle = v * 0.5; - - // TODO: optimize sin cos - - auto fcos = cos(half_angle); - auto fsin = sin(half_angle); - - // auto fsin = half_angle; - // auto fcos = 1.0 - half_angle * half_angle * 0.5; - - frame = Frame(Vector3(0.0, 0.0, 0.0), Quaternion(axis.x() * fsin, axis.y() * fsin, axis.z() * fsin, fcos)); - - break; - } - case moveit::core::JointModel::PRISMATIC: - { - auto axis = joint_axis_list[joint_index]; - auto v = variables[joint_model->getFirstVariableIndex()]; - frame = Frame(axis * v, Quaternion(0.0, 0.0, 0.0, 1.0)); - break; - } - case moveit::core::JointModel::FLOATING: - { - auto* vv = variables + joint_model->getFirstVariableIndex(); - frame.pos = Vector3(vv[0], vv[1], vv[2]); - frame.rot = Quaternion(vv[3], vv[4], vv[5], vv[6]).normalized(); - // LOG("floating", joint_model->getFirstVariableIndex(), vv[0], vv[1], vv[2], vv[3], vv[4], vv[5], vv[6]); - break; - } - default: - { - auto* joint_variables = variables + joint_model->getFirstVariableIndex(); - Eigen::Affine3d joint_transform; - joint_model->computeTransform(joint_variables, joint_transform); - frame = Frame(joint_transform); - break; - } - } - - // LOG("local joint frame", joint_model->getType(), joint_model->getName(), frame); - } - inline void getJointFrame(const moveit::core::JointModel* joint_model, const std::vector& variables, Frame& frame) { getJointFrame(joint_model, variables.data(), frame); } - -protected: - inline const Frame& getLinkFrame(const moveit::core::LinkModel* link_model) { return link_frames[link_model->getLinkIndex()]; } - - inline bool checkJointMoved(const moveit::core::JointModel* joint_model) - { - size_t i0 = joint_model->getFirstVariableIndex(); - size_t cnt = joint_model->getVariableCount(); - if(cnt == 0) return true; - if(cnt == 1) return !(variables[i0] == joint_cache_variables[i0]); - for(size_t i = i0; i < i0 + cnt; i++) - if(!(variables[i] == joint_cache_variables[i])) return true; - return false; - } - inline void putJointCache(const moveit::core::JointModel* joint_model, const Frame& frame) - { - joint_cache_frames[joint_model->getJointIndex()] = frame; - size_t i0 = joint_model->getFirstVariableIndex(); - size_t cnt = joint_model->getVariableCount(); - for(size_t i = i0; i < i0 + cnt; i++) - joint_cache_variables[i] = variables[i]; - } - inline const Frame& getJointFrame(const moveit::core::JointModel* joint_model) - { - size_t joint_index = joint_model->getJointIndex(); - - if(!checkJointMoved(joint_model)) return joint_cache_frames[joint_index]; - - getJointFrame(joint_model, variables, joint_cache_frames[joint_index]); - - size_t cnt = joint_model->getVariableCount(); - if(cnt) - { - size_t i0 = joint_model->getFirstVariableIndex(); - if(cnt == 1) - joint_cache_variables[i0] = variables[i0]; - else - for(size_t i = i0; i < i0 + cnt; i++) - joint_cache_variables[i] = variables[i]; - } - - // TODO: optimize copy - - /*size_t cnt = joint_model->getVariableCount(); - size_t i0 = joint_model->getFirstVariableIndex(); - memcpy(joint_cache_variables.data() + i0, variables.data() + i0, cnt * 8);*/ - - return joint_cache_frames[joint_index]; - } - -public: - RobotJointEvaluator(moveit::core::RobotModelConstPtr model) - : robot_model(model) - { - joint_cache_variables.clear(); - joint_cache_variables.resize(model->getVariableCount(), DBL_MAX); - - joint_cache_frames.clear(); - joint_cache_frames.resize(model->getJointModelCount()); - - link_frames.clear(); - for(auto* link_model : model->getLinkModels()) - link_frames.push_back(Frame(link_model->getJointOriginTransform())); - - joint_axis_list.clear(); - joint_axis_list.resize(robot_model->getJointModelCount()); - for(size_t i = 0; i < joint_axis_list.size(); i++) - { - auto* joint_model = robot_model->getJointModel(i); - if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); - if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); - } - } -}; - -// fast tree fk -class RobotFK_Fast_Base : protected RobotJointEvaluator -{ -protected: - std::vector tip_names; - std::vector tip_frames; - std::vector tip_links; - std::vector link_schedule; - std::vector global_frames; - std::vector> link_chains; - std::vector active_variables; - std::vector> variable_to_link_map; - std::vector variable_to_link_map_2; - std::vector> variable_to_link_chain_map; - inline void updateMimic(std::vector& values) - { - for(auto* joint : robot_model->getMimicJointModels()) - { - auto src = joint->getMimic()->getFirstVariableIndex(); - auto dest = joint->getFirstVariableIndex(); - - // TODO: more dimensions ??? - values[dest] = values[src] * joint->getMimicFactor() + joint->getMimicOffset(); - - // moveit doesn't seem to support multiple mimic dimensions either... ??? - - // TODO: ok??? - // for(size_t base = 0; base < joint->getVariableCount(); base++) - // values[base + dest] = values[base + src] * joint->getMimicFactor() + joint->getMimicOffset(); - } - } - -public: - RobotFK_Fast_Base(moveit::core::RobotModelConstPtr model) - : RobotJointEvaluator(model) - { - } - void initialize(const std::vector& tip_link_indices) - { - - tip_names.resize(tip_link_indices.size()); - for(size_t i = 0; i < tip_link_indices.size(); i++) - tip_names[i] = robot_model->getLinkModelNames()[tip_link_indices[i]]; - - tip_frames.resize(tip_names.size()); - - tip_links.clear(); - for(const auto& n : tip_names) - tip_links.push_back(robot_model->getLinkModel(n)); - - global_frames.resize(robot_model->getLinkModelCount()); - - link_chains.clear(); - link_schedule.clear(); - for(auto* tip_link : tip_links) - { - std::vector chain; - for(auto* link = tip_link; link; link = link->getParentLinkModel()) - chain.push_back(link); - reverse(chain.begin(), chain.end()); - link_chains.push_back(chain); - for(auto* link : chain) - { - if(find(link_schedule.begin(), link_schedule.end(), link) != link_schedule.end()) continue; - link_schedule.push_back(link); - } - } - - active_variables.clear(); - for(auto* link_model : link_schedule) - { - auto link_index = link_model->getLinkIndex(); - auto* joint_model = link_model->getParentJointModel(); - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - for(size_t vi = vstart; vi < vstart + vcount; vi++) - { - variable_to_link_map.push_back(std::make_pair(vi, link_index)); - - if(find(active_variables.begin(), active_variables.end(), vi) != active_variables.end()) continue; - active_variables.push_back(vi); - } - } - - variable_to_link_map_2.resize(robot_model->getVariableCount()); - for(auto* link_model : link_schedule) - { - auto link_index = link_model->getLinkIndex(); - auto* joint_model = link_model->getParentJointModel(); - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - for(size_t vi = vstart; vi < vstart + vcount; vi++) - { - variable_to_link_map_2[vi] = link_index; - } - } - - variable_to_link_chain_map.resize(robot_model->getVariableCount()); - for(size_t chain_index = 0; chain_index < link_chains.size(); chain_index++) - { - auto& link_chain = link_chains[chain_index]; - for(size_t ipos = 0; ipos < link_chain.size(); ipos++) - { - auto* link_model = link_chain[ipos]; - auto* joint_model = link_model->getParentJointModel(); - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - for(size_t vi = vstart; vi < vstart + vcount; vi++) - { - variable_to_link_chain_map[vi] = std::make_pair(chain_index, ipos); - } - } - } - } - void applyConfiguration(const std::vector& jj0) - { - FNPROFILER(); - variables = jj0; - updateMimic(variables); - for(auto* link_model : link_schedule) - { - auto* joint_model = link_model->getParentJointModel(); - auto* parent_link_model = joint_model->getParentLinkModel(); - if(parent_link_model) - { - concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), getJointFrame(joint_model), global_frames[link_model->getLinkIndex()]); - } - else - { - concat(getLinkFrame(link_model), getJointFrame(joint_model), global_frames[link_model->getLinkIndex()]); - } - // if(linkModel->getParentLinkModel() && linkModel->getParentLinkModel()->getLinkIndex() > linkModel->getLinkIndex()) { LOG("wrong link order"); throw runtime_error("wrong link order"); } - } - for(size_t itip = 0; itip < tip_links.size(); itip++) - { - tip_frames[itip] = global_frames[tip_links[itip]->getLinkIndex()]; - } - } - inline const Frame& getTipFrame(size_t fi) const { return tip_frames[fi]; } - inline const std::vector& getTipFrames() const { return tip_frames; } - inline void incrementalBegin(const std::vector& jj) { applyConfiguration(jj); } - inline void incrementalEnd() {} - inline const Frame& getJointVariableFrame(size_t variable_index) { return global_frames[variable_to_link_map_2[variable_index]]; } -}; - -// incremental tip frame updates without full recomputation -class RobotFK_Fast : public RobotFK_Fast_Base -{ -protected: - std::vector> chain_cache; - std::vector vars, variables0; - std::vector test_tips; - std::vector links_changed; - bool use_incremental; - bool last_use_incremental; - void updateIncremental(const std::vector& vars0) - { - FNPROFILER(); - - // PROFILER_COUNTER("incremental update count"); - - // init variable lists - vars = vars0; - updateMimic(vars); - for(auto& vi : active_variables) - { - // vars[vi] = vars0[vi]; - variables0[vi] = variables[vi]; - } - - // find moved links - for(auto* link_model : link_schedule) - links_changed[link_model->getLinkIndex()] = false; - for(auto& x : variable_to_link_map) - { - auto variable_index = x.first; - auto link_index = x.second; - if(vars[variable_index] != variables[variable_index]) links_changed[link_index] = true; - } - - // update - for(size_t ichain = 0; ichain < link_chains.size(); ichain++) - { - auto& link_chain = link_chains[ichain]; - auto& cache_chain = chain_cache[ichain]; - - size_t iposmax = 0; - for(size_t ipos = 0; ipos < link_chain.size(); ipos++) - { - auto* link_model = link_chain[ipos]; - bool changed = links_changed[link_model->getLinkIndex()]; - if(changed) iposmax = ipos; - } - - for(size_t ipos = 0; ipos <= iposmax; ipos++) - { - auto* link_model = link_chain[ipos]; - auto* joint_model = link_model->getParentJointModel(); - - bool changed = links_changed[link_model->getLinkIndex()]; - - if(cache_chain.size() <= ipos || changed) - { - Frame before, after; - - if(ipos < cache_chain.size()) - { - before = cache_chain[ipos]; - } - else - { - if(ipos > 0) - { - concat(cache_chain[ipos - 1], getLinkFrame(link_model), getJointFrame(joint_model), before); - } - else - { - concat(getLinkFrame(link_model), getJointFrame(joint_model), before); - } - } - - chain_cache[ichain].resize(ipos + 1, Frame::identity()); - - if(changed) - { - if(ichain > 0 && ipos < link_chains[ichain - 1].size() && link_chains[ichain][ipos] == link_chains[ichain - 1][ipos]) - { - after = chain_cache[ichain - 1][ipos]; - } - else - { - size_t vstart = joint_model->getFirstVariableIndex(); - size_t vcount = joint_model->getVariableCount(); - - if(vcount == 1) - variables[vstart] = vars[vstart]; - else - for(size_t vi = vstart; vi < vstart + vcount; vi++) - variables[vi] = vars[vi]; - - if(ipos > 0) - { - concat(cache_chain[ipos - 1], getLinkFrame(link_model), getJointFrame(joint_model), after); - } - else - { - concat(getLinkFrame(link_model), getJointFrame(joint_model), after); - } - - if(vcount == 1) - variables[vstart] = variables0[vstart]; - else - for(size_t vi = vstart; vi < vstart + vcount; vi++) - variables[vi] = variables0[vi]; - - // PROFILER_COUNTER("incremental update transforms"); - } - chain_cache[ichain][ipos] = after; - - // tipFrames[ichain] = inverse(before) * tipFrames[ichain]; - // tipFrames[ichain] = after * tipFrames[ichain]; - - /*Frame before_inverse; - invert(before, before_inverse); - - //tipFrames[ichain] = after * before_inverse * tipFrames[ichain]; - { - concat(after, before_inverse, tip_frames[ichain], tip_frames[ichain]); - }*/ - - change(after, before, tip_frames[ichain], tip_frames[ichain]); - } - else - { - after = before; - chain_cache[ichain][ipos] = after; - } - } - } - } - - // set new vars - // variables = vars; - for(auto& vi : active_variables) - variables[vi] = vars[vi]; - - // test - if(0) - { - test_tips = tip_frames; - RobotFK_Fast_Base::applyConfiguration(vars); - for(size_t i = 0; i < tip_frames.size(); i++) - { - auto dist = tip_frames[i].pos.distance(test_tips[i].pos); - LOG_VAR(dist); - if(dist > 0.001) - { - LOG(tip_frames[i].pos.x(), tip_frames[i].pos.y(), tip_frames[i].pos.z()); - LOG(test_tips[i].pos.x(), test_tips[i].pos.y(), test_tips[i].pos.z()); - ERROR("incremental update error"); - } - } - } - } - -public: - RobotFK_Fast(moveit::core::RobotModelConstPtr model) - : RobotFK_Fast_Base(model) - , use_incremental(false) - { - } - inline void incrementalBegin(const std::vector& jj) - { - applyConfiguration(jj); - chain_cache.clear(); - chain_cache.resize(tip_frames.size()); - links_changed.resize(robot_model->getLinkModelCount()); - vars.resize(jj.size()); - variables0.resize(jj.size()); - use_incremental = true; - } - inline void incrementalEnd() - { - use_incremental = false; - // applyConfiguration(variables); - } - inline void applyConfiguration(const std::vector& jj) - { - if(use_incremental) - updateIncremental(jj); - else - RobotFK_Fast_Base::applyConfiguration(jj); - } -}; - -// fast tree fk jacobian -class RobotFK_Jacobian : public RobotFK_Fast -{ - typedef RobotFK_Fast Base; - -protected: - std::vector> joint_dependencies; - std::vector tip_dependencies; - -public: - RobotFK_Jacobian(moveit::core::RobotModelConstPtr model) - : RobotFK_Fast(model) - { - } - void initialize(const std::vector& tip_link_indices) - { - Base::initialize(tip_link_indices); - auto tip_count = tip_names.size(); - joint_dependencies.resize(robot_model->getJointModelCount()); - for(auto& x : joint_dependencies) - x.clear(); - for(auto* link_model : link_schedule) - { - auto* joint_model = link_model->getParentJointModel(); - joint_dependencies[joint_model->getJointIndex()].push_back(joint_model); - } - for(auto* link_model : link_schedule) - { - auto* joint_model = link_model->getParentJointModel(); - if(auto* mimic = joint_model->getMimic()) - { - while(mimic->getMimic() && mimic->getMimic() != joint_model) - mimic = mimic->getMimic(); - joint_dependencies[mimic->getJointIndex()].push_back(joint_model); - } - } - tip_dependencies.resize(robot_model->getJointModelCount() * tip_count); - for(auto& x : tip_dependencies) - x = 0; - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - for(auto* link_model = tip_links[tip_index]; link_model; link_model = link_model->getParentLinkModel()) - { - auto* joint_model = link_model->getParentJointModel(); - tip_dependencies[joint_model->getJointIndex() * tip_count + tip_index] = 1; - } - } - } - void computeJacobian(const std::vector& variable_indices, Eigen::MatrixXd& jacobian) - { - double step_size = 0.00001; - double half_step_size = step_size * 0.5; - double inv_step_size = 1.0 / step_size; - auto tip_count = tip_frames.size(); - jacobian.resize(tip_count * 6, variable_indices.size()); - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - for(size_t itip = 0; itip < tip_count; itip++) - { - jacobian(itip * 6 + 0, icol) = 0; - jacobian(itip * 6 + 1, icol) = 0; - jacobian(itip * 6 + 2, icol) = 0; - jacobian(itip * 6 + 3, icol) = 0; - jacobian(itip * 6 + 4, icol) = 0; - jacobian(itip * 6 + 5, icol) = 0; - } - } - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - auto ivar = variable_indices[icol]; - auto* var_joint_model = robot_model->getJointOfVariable(ivar); - if(var_joint_model->getMimic()) continue; - for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) - { - double scale = 1; - for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) - { - scale *= m->getMimicFactor(); - } - auto* link_model = joint_model->getChildLinkModel(); - switch(joint_model->getType()) - { - case moveit::core::JointModel::FIXED: - { - // fixed joint: zero gradient (do nothing) - continue; - } - case moveit::core::JointModel::REVOLUTE: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - - auto& tip_frame = tip_frames[itip]; - - auto q = link_frame.rot.inverse() * tip_frame.rot; - q = q.inverse(); - - auto rot = joint_axis_list[joint_model->getJointIndex()]; - quat_mul_vec(q, rot, rot); - - auto vel = link_frame.pos - tip_frame.pos; - quat_mul_vec(tip_frame.rot.inverse(), vel, vel); - - vel = vel.cross(rot); - - jacobian(itip * 6 + 0, icol) += vel.x() * scale; - jacobian(itip * 6 + 1, icol) += vel.y() * scale; - jacobian(itip * 6 + 2, icol) += vel.z() * scale; - - jacobian(itip * 6 + 3, icol) += rot.x() * scale; - jacobian(itip * 6 + 4, icol) += rot.y() * scale; - jacobian(itip * 6 + 5, icol) += rot.z() * scale; - - // LOG("a", vel.x(), vel.y(), vel.z(), rot.x(), rot.y(), rot.z()); - } - continue; - } - case moveit::core::JointModel::PRISMATIC: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - - auto& tip_frame = tip_frames[itip]; - - auto q = link_frame.rot.inverse() * tip_frame.rot; - q = q.inverse(); - - auto axis = joint_axis_list[joint_model->getJointIndex()]; - auto v = axis; - quat_mul_vec(q, axis, v); - - jacobian(itip * 6 + 0, icol) += v.x() * scale; - jacobian(itip * 6 + 1, icol) += v.y() * scale; - jacobian(itip * 6 + 2, icol) += v.z() * scale; - - // LOG("a", v.x(), v.y(), v.z(), 0, 0, 0); - } - continue; - } - default: - { - // numeric differentiation for joint types that are not yet implemented / or joint types that might be added to moveit in the future - auto ivar2 = ivar; - if(joint_model->getMimic()) ivar2 = ivar2 - var_joint_model->getFirstVariableIndex() + joint_model->getFirstVariableIndex(); - auto& link_frame_1 = global_frames[link_model->getLinkIndex()]; - auto v0 = variables[ivar2]; - // auto joint_frame_1 = getJointFrame(joint_model); - variables[ivar2] = v0 + step_size; - auto joint_frame_2 = getJointFrame(joint_model); - variables[ivar2] = v0; - Frame link_frame_2; - if(auto* parent_link_model = joint_model->getParentLinkModel()) - concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), joint_frame_2, link_frame_2); - else - concat(getLinkFrame(link_model), joint_frame_2, link_frame_2); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - auto tip_frame_1 = tip_frames[itip]; - Frame tip_frame_2; - change(link_frame_2, link_frame_1, tip_frame_1, tip_frame_2); - auto twist = frameTwist(tip_frame_1, tip_frame_2); - jacobian(itip * 6 + 0, icol) += twist.vel.x() * inv_step_size * scale; - jacobian(itip * 6 + 1, icol) += twist.vel.y() * inv_step_size * scale; - jacobian(itip * 6 + 2, icol) += twist.vel.z() * inv_step_size * scale; - jacobian(itip * 6 + 3, icol) += twist.rot.x() * inv_step_size * scale; - jacobian(itip * 6 + 4, icol) += twist.rot.y() * inv_step_size * scale; - jacobian(itip * 6 + 5, icol) += twist.rot.z() * inv_step_size * scale; - } - continue; - } - } - } - } - } -}; - -#if 0 - -struct RobotFK : public RobotFK_Jacobian -{ - std::vector mutation_variable_indices; - std::vector mutation_initial_variable_positions; - std::vector mutation_temp; - RobotFK(moveit::core::RobotModelConstPtr model) - : RobotFK_Jacobian(model) - { - } - void initializeMutationApproximator(const std::vector& variable_indices) - { - mutation_variable_indices = variable_indices; - mutation_initial_variable_positions = variables; - } - void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) - { - mutation_temp = mutation_initial_variable_positions; - mutation_temp[variable_index] += variable_delta; - applyConfiguration(mutation_temp); - output.clear(); - for(auto& f : tip_frames) output.push_back(f); - } - void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) - { - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t mutation_index = 0; mutation_index < mutation_count; mutation_index++) - { - mutation_temp = mutation_initial_variable_positions; - for(size_t i = 0; i < mutation_variable_indices.size(); i++) - { - mutation_temp[mutation_variable_indices[i]] = mutation_values[mutation_index][i]; - } - applyConfiguration(mutation_temp); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - tip_frame_mutations[mutation_index][tip_index] = tip_frames[tip_index]; - } - } - } -}; - -#else - -#define USE_QUADRATIC_EXTRAPOLATION 0 - -class RobotFK_Mutator : public RobotFK_Jacobian -{ - typedef RobotFK_Jacobian Base; - Eigen::MatrixXd mutation_approx_jacobian; - std::vector> mutation_approx_frames; -#if USE_QUADRATIC_EXTRAPOLATION - std::vector> mutation_approx_quadratics; -#endif - std::vector mutation_approx_variable_indices; - std::vector> mutation_approx_mask; - std::vector> mutation_approx_map; - aligned_vector tip_frames_aligned; - -public: - RobotFK_Mutator(moveit::core::RobotModelConstPtr model) - : RobotFK_Jacobian(model) - { - } - - void initializeMutationApproximator(const std::vector& variable_indices) - { - FNPROFILER(); - - mutation_approx_variable_indices = variable_indices; - - auto tip_count = tip_names.size(); - - tip_frames_aligned.resize(tip_frames.size()); - for(size_t i = 0; i < tip_frames.size(); i++) - tip_frames_aligned[i] = tip_frames[i]; - - // init first order approximators - { - if(mutation_approx_frames.size() < tip_count) mutation_approx_frames.resize(tip_count); - - for(size_t itip = 0; itip < tip_count; itip++) - mutation_approx_frames[itip].resize(robot_model->getVariableCount()); - - for(size_t itip = 0; itip < tip_count; itip++) - for(auto ivar : variable_indices) - mutation_approx_frames[itip][ivar] = Frame::identity(); - - computeJacobian(variable_indices, mutation_approx_jacobian); - - for(size_t icol = 0; icol < variable_indices.size(); icol++) - { - size_t ivar = variable_indices[icol]; - for(size_t itip = 0; itip < tip_count; itip++) - { - { - Vector3 t; - t.setX(mutation_approx_jacobian(itip * 6 + 0, icol)); - t.setY(mutation_approx_jacobian(itip * 6 + 1, icol)); - t.setZ(mutation_approx_jacobian(itip * 6 + 2, icol)); - quat_mul_vec(tip_frames[itip].rot, t, t); - mutation_approx_frames[itip][ivar].pos = t; - } - - { - Quaternion q; - q.setX(mutation_approx_jacobian(itip * 6 + 3, icol) * 0.5); - q.setY(mutation_approx_jacobian(itip * 6 + 4, icol) * 0.5); - q.setZ(mutation_approx_jacobian(itip * 6 + 5, icol) * 0.5); - q.setW(1.0); - quat_mul_quat(tip_frames[itip].rot, q, q); - q -= tip_frames[itip].rot; - mutation_approx_frames[itip][ivar].rot = q; - } - } - } - } - -// init second order approximators -#if USE_QUADRATIC_EXTRAPOLATION - { - if(mutation_approx_quadratics.size() < tip_count) mutation_approx_quadratics.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - mutation_approx_quadratics[itip].resize(robot_model->getVariableCount()); - for(size_t itip = 0; itip < tip_count; itip++) - for(auto ivar : variable_indices) - mutation_approx_quadratics[itip][ivar].pos = Vector3(0, 0, 0); - for(auto ivar : variable_indices) - { - auto* var_joint_model = robot_model->getJointOfVariable(ivar); - if(var_joint_model->getMimic()) continue; - for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) - { - double scale = 1; - for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) - { - scale *= m->getMimicFactor(); - } - auto* link_model = joint_model->getChildLinkModel(); - switch(joint_model->getType()) - { - case moveit::core::JointModel::REVOLUTE: - { - auto& link_frame = global_frames[link_model->getLinkIndex()]; - for(size_t itip = 0; itip < tip_count; itip++) - { - if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; - - auto& tip_frame = tip_frames[itip]; - - auto axis = joint_axis_list[joint_model->getJointIndex()]; - quat_mul_vec(link_frame.rot, axis, axis); - - auto v = link_frame.pos - tip_frame.pos; - v -= axis * v.dot(axis); - - mutation_approx_quadratics[itip][ivar].pos = v * 0.5 * (scale * scale); - } - continue; - } - default: - { - continue; - } - } - } - } - } -#endif - - // init mask - if(mutation_approx_mask.size() < tip_count) mutation_approx_mask.resize(tip_count); - if(mutation_approx_map.size() < tip_count) mutation_approx_map.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip].size() < robot_model->getVariableCount()) mutation_approx_mask[itip].resize(robot_model->getVariableCount()); - mutation_approx_map[itip].clear(); - for(size_t ii = 0; ii < variable_indices.size(); ii++) - // for(size_t ivar : variable_indices) - { - auto ivar = variable_indices[ii]; - auto& frame = mutation_approx_frames[itip][ivar]; - bool b = false; - b |= (frame.pos.x() != 0.0); - b |= (frame.pos.y() != 0.0); - b |= (frame.pos.z() != 0.0); - b |= (frame.rot.x() != 0.0); - b |= (frame.rot.y() != 0.0); - b |= (frame.rot.z() != 0.0); - mutation_approx_mask[itip][ivar] = b; - if(b) mutation_approx_map[itip].push_back(ii); - } - } - } - -#if FUNCTION_MULTIVERSIONING - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const - { - // BLOCKPROFILER("computeApproximateMutations C"); - auto tip_count = tip_names.size(); - output.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip][variable_index] == 0) continue; - auto& joint_delta = mutation_approx_frames[itip][variable_index]; - const Frame& tip_frame = input[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; - __m256d p = _mm256_load_pd(tip_frame_ptr + 0); - __m256d r = _mm256_load_pd(tip_frame_ptr + 4); - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); - __m256d ff = _mm256_set1_pd(variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); - } -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - } -#endif - auto& tip_mutation = output[itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm256_store_pd(tip_mutation_ptr + 0, p); - _mm256_store_pd(tip_mutation_ptr + 4, r); - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const - { - // BLOCKPROFILER("computeApproximateMutations C"); - auto tip_count = tip_names.size(); - output.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip][variable_index] == 0) continue; - auto& joint_delta = mutation_approx_frames[itip][variable_index]; - const Frame& tip_frame = input[itip]; - const double* tip_frame_ptr = (const double*)&tip_frame; - __m128d pxy = _mm_load_pd(tip_frame_ptr + 0); - __m128d pzw = _mm_load_sd(tip_frame_ptr + 2); - __m128d rxy = _mm_load_pd(tip_frame_ptr + 4); - __m128d rzw = _mm_load_pd(tip_frame_ptr + 6); - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); - __m128d ff = _mm_set1_pd(variable_delta); - pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); - pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); - rxy = _mm_add_pd(rxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 4))); - rzw = _mm_add_pd(rzw, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 6))); - } -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m128d ff = _mm_set1_pd(variable_delta * variable_delta); - pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); - pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); - } -#endif - auto& tip_mutation = output[itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm_store_pd(tip_mutation_ptr + 0, pxy); - _mm_store_sd(tip_mutation_ptr + 2, pzw); - _mm_store_pd(tip_mutation_ptr + 4, rxy); - _mm_store_pd(tip_mutation_ptr + 6, rzw); - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("default"))) -#endif - void - computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const - { - // BLOCKPROFILER("computeApproximateMutations C"); - auto tip_count = tip_names.size(); - output.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - if(mutation_approx_mask[itip][variable_index] == 0) continue; - - auto& joint_delta = mutation_approx_frames[itip][variable_index]; - - const Frame& tip_frame = input[itip]; - - double px = tip_frame.pos.x(); - double py = tip_frame.pos.y(); - double pz = tip_frame.pos.z(); - - double rx = tip_frame.rot.x(); - double ry = tip_frame.rot.y(); - double rz = tip_frame.rot.z(); - double rw = tip_frame.rot.w(); - - px += joint_delta.pos.x() * variable_delta; - py += joint_delta.pos.y() * variable_delta; - pz += joint_delta.pos.z() * variable_delta; - -#if USE_QUADRATIC_EXTRAPOLATION - double variable_delta_sq = variable_delta * variable_delta; - px += mutation_approx_quadratics[itip][variable_index].pos.x() * variable_delta_sq; - py += mutation_approx_quadratics[itip][variable_index].pos.y() * variable_delta_sq; - pz += mutation_approx_quadratics[itip][variable_index].pos.z() * variable_delta_sq; -#endif - - rx += joint_delta.rot.x() * variable_delta; - ry += joint_delta.rot.y() * variable_delta; - rz += joint_delta.rot.z() * variable_delta; - rw += joint_delta.rot.w() * variable_delta; - - auto& tip_mutation = output[itip]; - - tip_mutation.pos.setX(px); - tip_mutation.pos.setY(py); - tip_mutation.pos.setZ(pz); - - tip_mutation.rot.setX(rx); - tip_mutation.rot.setY(ry); - tip_mutation.rot.setZ(rz); - tip_mutation.rot.setW(rw); - } - } - -#if FUNCTION_MULTIVERSIONING - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const - { - const double* p_variables = variables.data(); - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - auto& joint_deltas = mutation_approx_frames[itip]; - - const Frame& tip_frame = tip_frames_aligned[itip]; - - const double* tip_frame_ptr = (const double*)&tip_frame; - __m256d p0 = _mm256_load_pd(tip_frame_ptr + 0); - __m256d r0 = _mm256_load_pd(tip_frame_ptr + 4); - - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - auto p = p0; - auto r = r0; - - for(size_t vii : mutation_approx_map[itip]) - { - size_t variable_index = mutation_approx_variable_indices[vii]; - double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; - - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); - __m256d ff = _mm256_set1_pd(variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); - } - -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); - p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); - } -#endif - } - - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm256_store_pd(tip_mutation_ptr + 0, p); - _mm256_store_pd(tip_mutation_ptr + 4, r); - } - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const - { - const double* p_variables = variables.data(); - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - auto& joint_deltas = mutation_approx_frames[itip]; - - const Frame& tip_frame = tip_frames_aligned[itip]; - - const double* tip_frame_ptr = (const double*)&tip_frame; - __m128d pxy0 = _mm_load_pd(tip_frame_ptr + 0); - __m128d pzw0 = _mm_load_sd(tip_frame_ptr + 2); - __m128d rxy0 = _mm_load_pd(tip_frame_ptr + 4); - __m128d rzw0 = _mm_load_pd(tip_frame_ptr + 6); - - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - auto pxy = pxy0; - auto pzw = pzw0; - auto rxy = rxy0; - auto rzw = rzw0; - - for(size_t vii : mutation_approx_map[itip]) - { - size_t variable_index = mutation_approx_variable_indices[vii]; - double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; - - { - auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); - __m128d ff = _mm_set1_pd(variable_delta); - pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); - pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); - rxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 4)), rxy); - rzw = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 6)), rzw); - } - -#if USE_QUADRATIC_EXTRAPOLATION - { - auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); - __m128d ff = _mm_set1_pd(variable_delta * variable_delta); - pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); - pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); - } -#endif - } - - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; - _mm_store_pd(tip_mutation_ptr + 0, pxy); - _mm_store_sd(tip_mutation_ptr + 2, pzw); - _mm_store_pd(tip_mutation_ptr + 4, rxy); - _mm_store_pd(tip_mutation_ptr + 6, rzw); - } - } - } - - __attribute__((hot)) __attribute__((noinline)) __attribute__((target("default"))) -#endif - void - computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const - { - const double* p_variables = variables.data(); - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t itip = 0; itip < tip_count; itip++) - { - auto& joint_deltas = mutation_approx_frames[itip]; - - const Frame& tip_frame = tip_frames[itip]; - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - double px = tip_frame.pos.x(); - double py = tip_frame.pos.y(); - double pz = tip_frame.pos.z(); - - double rx = tip_frame.rot.x(); - double ry = tip_frame.rot.y(); - double rz = tip_frame.rot.z(); - double rw = tip_frame.rot.w(); - - for(size_t vii : mutation_approx_map[itip]) - { - size_t variable_index = mutation_approx_variable_indices[vii]; - - double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; - - px += joint_deltas[variable_index].pos.x() * variable_delta; - py += joint_deltas[variable_index].pos.y() * variable_delta; - pz += joint_deltas[variable_index].pos.z() * variable_delta; - -#if USE_QUADRATIC_EXTRAPOLATION - double variable_delta_sq = variable_delta * variable_delta; - px += mutation_approx_quadratics[itip][variable_index].pos.x() * variable_delta_sq; - py += mutation_approx_quadratics[itip][variable_index].pos.y() * variable_delta_sq; - pz += mutation_approx_quadratics[itip][variable_index].pos.z() * variable_delta_sq; -#endif - - rx += joint_deltas[variable_index].rot.x() * variable_delta; - ry += joint_deltas[variable_index].rot.y() * variable_delta; - rz += joint_deltas[variable_index].rot.z() * variable_delta; - rw += joint_deltas[variable_index].rot.w() * variable_delta; - } - - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - - tip_mutation.pos.setX(px); - tip_mutation.pos.setY(py); - tip_mutation.pos.setZ(pz); - - tip_mutation.rot.setX(rx); - tip_mutation.rot.setY(ry); - tip_mutation.rot.setZ(rz); - tip_mutation.rot.setW(rw); - } - } - } -}; - -#if 0 -class RobotFK_Mutator_2 : public RobotFK_Mutator -{ - struct JointApprox - { - ssize_t mutation_index; - ssize_t variable_index; - Vector3 delta_position; - Vector3 delta_rotation; - size_t link_index; - size_t parent_link_index; - Vector3 link_position; - }; - std::vector joint_approximators; - - struct LinkApprox - { - Vector3 position; - Vector3 rotation; - }; - std::vector link_approximators; - - std::vector link_approx_mask; - - std::vector> variable_to_approximator_index_map; - -public: - RobotFK_Mutator_2(moveit::core::RobotModelConstPtr model) - : RobotFK_Mutator(model) - { - } - - void initializeMutationApproximator(const std::vector& variable_indices) - { - RobotFK_Mutator::initializeMutationApproximator(variable_indices); - - BLOCKPROFILER("initializeMutationApproximator tree"); - - auto tip_count = tip_names.size(); - - link_approximators.resize(robot_model->getLinkModelCount() + 1); - for(auto& l : link_approximators) - { - l.position = Vector3(0, 0, 0); - l.rotation = Vector3(0, 0, 0); - } - - link_approx_mask.clear(); - link_approx_mask.resize(robot_model->getLinkModelCount(), 0); - - joint_approximators.clear(); - - // build joint approximators - for(size_t imut = 0; imut < variable_indices.size(); imut++) - { - size_t ivar = variable_indices[imut]; - auto* joint_model = robot_model->getJointOfVariable(ivar); - { - auto* link_model = joint_model->getChildLinkModel(); - // if(link_approx_mask[link_model->getLinkIndex()]) continue; - joint_approximators.emplace_back(); - auto& joint = joint_approximators.back(); - joint.mutation_index = imut; - joint.variable_index = ivar; - joint.link_index = link_model->getLinkIndex(); - link_approx_mask[link_model->getLinkIndex()] = 1; - } - for(auto* mimic_joint_model : joint_model->getMimicRequests()) - { - // LOG("mimic", mimic_joint_model); - auto* joint_model = mimic_joint_model; - auto* link_model = joint_model->getChildLinkModel(); - // if(link_approx_mask[link_model->getLinkIndex()]) continue; - joint_approximators.emplace_back(); - auto& joint = joint_approximators.back(); - joint.mutation_index = imut; - joint.variable_index = ivar; - joint.link_index = link_model->getLinkIndex(); - link_approx_mask[link_model->getLinkIndex()] = 1; - } - } - - for(auto& joint : joint_approximators) - { - // size_t imut = joint.mutation_index; - // size_t ivar = joint.variable_index; - auto* link_model = robot_model->getLinkModel(joint.link_index); - auto* joint_model = link_model->getParentJointModel(); - joint.delta_position = Vector3(0, 0, 0); - joint.delta_rotation = Vector3(0, 0, 0); - auto& link_frame = global_frames[link_model->getLinkIndex()]; - switch(joint_model->getType()) - { - case moveit::core::JointModel::REVOLUTE: - quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_rotation); - break; - case moveit::core::JointModel::PRISMATIC: - quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_position); - break; - } - for(auto* j = joint_model; j->getMimic(); j = j->getMimic()) - { - joint.delta_rotation *= j->getMimicFactor(); - joint.delta_position *= j->getMimicFactor(); - break; - } - } - - // continue extrapolation to tip frames, if not already done - for(auto* link_model : tip_links) - { - if(link_approx_mask[link_model->getLinkIndex()]) continue; - joint_approximators.emplace_back(); - auto& joint = joint_approximators.back(); - joint.mutation_index = 0; - joint.variable_index = 0; - joint.link_index = link_model->getLinkIndex(); - joint.delta_position = Vector3(0, 0, 0); - joint.delta_rotation = Vector3(0, 0, 0); - link_approx_mask[link_model->getLinkIndex()] = 1; - } - - std::sort(joint_approximators.begin(), joint_approximators.end(), [](const JointApprox& a, const JointApprox& b) { return a.link_index < b.link_index; }); - - for(auto& joint : joint_approximators) - { - for(auto* link_model = robot_model->getLinkModel(joint.link_index); link_model;) - { - if(link_approx_mask[link_model->getLinkIndex()] >= 2) - { - joint.parent_link_index = link_model->getLinkIndex(); - joint.link_position = global_frames[joint.link_index].pos - global_frames[joint.parent_link_index].pos; - link_approx_mask[joint.link_index] = 2; - break; - } - link_model = link_model->getParentLinkModel(); - if(link_model == 0) - { - joint.parent_link_index = robot_model->getLinkModelCount(); - joint.link_position = global_frames[joint.link_index].pos; - link_approx_mask[joint.link_index] = 2; - break; - } - } - } - - variable_to_approximator_index_map.resize(robot_model->getVariableCount()); - for(auto& l : variable_to_approximator_index_map) - l.clear(); - for(size_t approximator_index = 0; approximator_index < joint_approximators.size(); approximator_index++) - { - if(joint_approximators[approximator_index].variable_index < 0) continue; - variable_to_approximator_index_map[joint_approximators[approximator_index].variable_index].push_back(approximator_index); - } - } - - void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) - { - auto tip_count = tip_names.size(); - tip_frame_mutations.resize(mutation_count); - for(auto& m : tip_frame_mutations) - m.resize(tip_count); - for(size_t imutation = 0; imutation < mutation_count; imutation++) - { - for(auto& joint : joint_approximators) - { - double dvar; - /*if(joint.mutation_index < 0) - dvar = 0; - else*/ - dvar = mutation_values[imutation][joint.mutation_index] - variables[joint.variable_index]; - - Vector3 dpos = joint.delta_position * dvar + joint.link_position; - - auto& parent = link_approximators[joint.parent_link_index]; - - // if(dpos.x() || dpos.y() || dpos.z()) - { - Vector3 dpos1 = Vector3(parent.rotation.y() * dpos.z() - parent.rotation.z() * dpos.y(), parent.rotation.z() * dpos.x() - parent.rotation.x() * dpos.z(), parent.rotation.x() * dpos.y() - parent.rotation.y() * dpos.x()); - - /*double xx = 0.5 * parent.rotation.x() * parent.rotation.x(); - double yy = 0.5 * parent.rotation.y() * parent.rotation.y(); - double zz = 0.5 * parent.rotation.z() * parent.rotation.z(); - - Vector3 dpos2 = Vector3( - dpos.x() * (yy + zz), - dpos.y() * (xx + zz), - dpos.z() * (xx + yy) - );*/ - - dpos += dpos1; - } - - // dpos -= dpos2; - - auto& link = link_approximators[joint.link_index]; - link.position = parent.position + dpos; - link.rotation = parent.rotation + joint.delta_rotation * dvar; - } - - for(size_t itip = 0; itip < tip_count; itip++) - { - const Frame& tip_frame = tip_frames[itip]; - auto& tip_mutation = tip_frame_mutations[imutation][itip]; - auto& link = link_approximators[tip_links[itip]->getLinkIndex()]; - tip_mutation.pos = link.position; - - tip_mutation.rot = tf::Quaternion(link.rotation.x() * 0.5, link.rotation.y() * 0.5, link.rotation.z() * 0.5, 1.0) * tip_frame.rot; - - /*tip_mutation.rot = tf::Quaternion( - link.rotation.x() * 0.5, - link.rotation.y() * 0.5, - link.rotation.z() * 0.5, - 1.0 - - link.rotation.length2() * 0.125 - ) * tip_frame.rot;*/ - } - } - } -}; - -typedef RobotFK_Mutator_2 RobotFK; - -#else - -typedef RobotFK_Mutator RobotFK; - -#endif - -#endif - -// for comparison -class RobotFK_MoveIt -{ - moveit::core::RobotModelConstPtr robot_model; - moveit::core::RobotState robot_state; - std::vector tipFrames; - std::vector tipLinks; - std::vector jj; - -public: - RobotFK_MoveIt(moveit::core::RobotModelConstPtr model) - : robot_model(model) - , robot_state(model) - { - } - void initialize(const std::vector& tip_link_indices) - { - tipFrames.resize(tip_link_indices.size()); - tipLinks.resize(tip_link_indices.size()); - for(size_t i = 0; i < tip_link_indices.size(); i++) - tipLinks[i] = robot_model->getLinkModel(tip_link_indices[i]); - } - void applyConfiguration(const std::vector& jj0) - { - BLOCKPROFILER("RobotFK_MoveIt applyConfiguration"); - jj = jj0; - robot_model->interpolate(jj0.data(), jj0.data(), 0.5, jj.data()); // force mimic update - robot_state.setVariablePositions(jj); - robot_state.update(); - for(size_t i = 0; i < tipFrames.size(); i++) - tipFrames[i] = Frame(robot_state.getGlobalLinkTransform(tipLinks[i])); - } - inline void incrementalBegin(const std::vector& jj) {} - inline void incrementalEnd() {} - const Frame& getTipFrame(size_t fi) const { return tipFrames[fi]; } - const std::vector& getTipFrames() const { return tipFrames; } -}; -} diff --git a/src/bio_ik-kinetic-support/src/goal_types.cpp b/src/bio_ik-kinetic-support/src/goal_types.cpp deleted file mode 100644 index a642a58..0000000 --- a/src/bio_ik-kinetic-support/src/goal_types.cpp +++ /dev/null @@ -1,269 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include - -#include -#include - -#include - -namespace bio_ik -{ - -void TouchGoal::describe(GoalContext& context) const -{ - LinkGoalBase::describe(context); - auto* robot_model = &context.getRobotModel(); - { - static std::map collision_cache; - if(collision_cache.find(robot_model) == collision_cache.end()) collision_cache[&context.getRobotModel()] = new CollisionModel(); - collision_model = collision_cache[robot_model]; - collision_model->collision_links.resize(robot_model->getLinkModelCount()); - } - link_model = robot_model->getLinkModel(this->getLinkName()); - size_t link_index = link_model->getLinkIndex(); - auto touch_goal_normal = normal.normalized(); - // auto fbrot = fb.rot.normalized(); - auto& collision_link = collision_model->collision_links[link_index]; - if(!collision_link.initialized) - { - collision_link.initialized = true; - collision_link.shapes.resize(link_model->getShapes().size()); - for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++) - { - collision_link.shapes[shape_index] = std::make_shared(); - auto& s = *collision_link.shapes[shape_index]; - s.frame = Frame(link_model->getCollisionOriginTransforms()[shape_index]); - auto* shape = link_model->getShapes()[shape_index].get(); - // LOG(link_model->getName(), shape_index, link_model->getShapes().size(), typeid(*shape).name()); - if(auto* mesh = dynamic_cast(shape)) - { - struct : bodies::ConvexMesh - { - std::vector points; - std::vector polygons; - std::vector plane_normals; - std::vector plane_dis; - void init(const shapes::Shape* shape) - { - type_ = shapes::MESH; - scaled_vertices_ = 0; - { - static std::mutex mutex; - std::lock_guard lock(mutex); - setDimensions(shape); - } - for(auto& v : mesh_data_->vertices_) - points.emplace_back(v.x(), v.y(), v.z()); - for(size_t triangle_index = 0; triangle_index < mesh_data_->triangles_.size() / 3; triangle_index++) - { - polygons.push_back(3); - polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 0]); - polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 1]); - polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 2]); - } - for(size_t triangle_index = 0; triangle_index < mesh_data_->triangles_.size() / 3; triangle_index++) - { - auto plane_index = mesh_data_->plane_for_triangle_[triangle_index]; - auto plane = mesh_data_->planes_[plane_index]; - plane_normals.emplace_back(plane.x(), plane.y(), plane.z()); - plane_dis.push_back(plane.w()); - } - } - } convex; - convex.init(mesh); - s.points = convex.points; - s.polygons = convex.polygons; - s.plane_normals = convex.plane_normals; - s.plane_dis = convex.plane_dis; - - // auto* fcl = new fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data()); - - // workaround for fcl::Convex initialization bug - auto* fcl = (fcl::Convex*)::operator new(sizeof(fcl::Convex)); - fcl->num_points = s.points.size(); - fcl = new(fcl) fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data()); - - s.geometry = decltype(s.geometry)(new collision_detection::FCLGeometry(fcl, link_model, shape_index)); - s.edges.resize(s.points.size()); - std::vector> edge_sets(s.points.size()); - for(size_t edge_index = 0; edge_index < fcl->num_edges; edge_index++) - { - auto edge = fcl->edges[edge_index]; - if(edge_sets[edge.first].find(edge.second) == edge_sets[edge.first].end()) - { - edge_sets[edge.first].insert(edge.second); - s.edges[edge.first].push_back(edge.second); - } - if(edge_sets[edge.second].find(edge.first) == edge_sets[edge.second].end()) - { - edge_sets[edge.second].insert(edge.first); - s.edges[edge.second].push_back(edge.first); - } - } - for(auto& p : s.points) - s.vertices.emplace_back(p[0], p[1], p[2]); - } - else - { - s.geometry = collision_detection::createCollisionGeometry(link_model->getShapes()[shape_index], link_model, shape_index); - } - // LOG("b"); - } - // getchar(); - } -} - -double TouchGoal::evaluate(const GoalContext& context) const -{ - double dmin = DBL_MAX; - context.getTempVector().resize(1); - auto& last_collision_vertex = context.getTempVector()[0]; - auto& fb = context.getLinkFrame(); - size_t link_index = link_model->getLinkIndex(); - auto& collision_link = collision_model->collision_links[link_index]; - for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++) - { - if(!collision_link.shapes[shape_index]->geometry) continue; - auto* shape = link_model->getShapes()[shape_index].get(); - // LOG(shape_index, typeid(*shape).name()); - if(auto* mesh = dynamic_cast(shape)) - { - auto& s = collision_link.shapes[shape_index]; - double d = DBL_MAX; - auto goal_normal = normal; - quat_mul_vec(fb.rot.inverse(), goal_normal, goal_normal); - quat_mul_vec(s->frame.rot.inverse(), goal_normal, goal_normal); - /*{ - size_t array_index = 0; - for(size_t vertex_index = 0; vertex_index < mesh->vertex_count; vertex_index++) - { - double dot_x = mesh->vertices[array_index++] * goal_normal.x(); - double dot_y = mesh->vertices[array_index++] * goal_normal.y(); - double dot_z = mesh->vertices[array_index++] * goal_normal.z(); - double e = dot_x + dot_y + dot_z; - if(e < d) d = e; - } - }*/ - if(mesh->vertex_count > 0) - { - size_t vertex_index = last_collision_vertex; - double vertex_dot_normal = goal_normal.dot(s->vertices[vertex_index]); - // size_t loops = 0; - while(true) - { - bool repeat = false; - for(auto vertex_index_2 : s->edges[vertex_index]) - { - auto vertex_dot_normal_2 = goal_normal.dot(s->vertices[vertex_index_2]); - if(vertex_dot_normal_2 < vertex_dot_normal) - { - vertex_index = vertex_index_2; - vertex_dot_normal = vertex_dot_normal_2; - repeat = true; - break; - } - } - if(!repeat) break; - // loops++; - } - // LOG_VAR(loops); - d = vertex_dot_normal; - last_collision_vertex = vertex_index; - } - d -= normal.dot(position - fb.pos); - // ROS_INFO("touch goal"); - if(d < dmin) dmin = d; - } - else - { - double offset = 10000; - static fcl::Sphere shape1(offset); - fcl::DistanceRequest request; - fcl::DistanceResult result; - auto pos1 = position - normal * offset * 2; - auto* shape2 = collision_link.shapes[shape_index]->geometry->collision_geometry_.get(); - auto frame2 = Frame(fb.pos, fb.rot.normalized()) * collision_link.shapes[shape_index]->frame; - double d = fcl::distance(&shape1, fcl::Transform3f(fcl::Vec3f(pos1.x(), pos1.y(), pos1.z())), shape2, fcl::Transform3f(fcl::Quaternion3f(frame2.rot.w(), frame2.rot.x(), frame2.rot.y(), frame2.rot.z()), fcl::Vec3f(frame2.pos.x(), frame2.pos.y(), frame2.pos.z())), request, result); - d -= offset; - if(d < dmin) dmin = d; - } - } - return dmin * dmin; -} - -void BalanceGoal::describe(GoalContext& context) const -{ - Goal::describe(context); - balance_infos.clear(); - double total = 0.0; - for(auto& link_name : context.getRobotModel().getLinkModelNames()) - { - auto link_urdf = context.getRobotModel().getURDF()->getLink(link_name); - if(!link_urdf) continue; - if(!link_urdf->inertial) continue; - const auto& center_urdf = link_urdf->inertial->origin.position; - tf::Vector3 center(center_urdf.x, center_urdf.y, center_urdf.z); - double mass = link_urdf->inertial->mass; - if(!(mass > 0)) continue; - balance_infos.emplace_back(); - balance_infos.back().center = center; - balance_infos.back().weight = mass; - total += mass; - context.addLink(link_name); - } - for(auto& b : balance_infos) - { - b.weight /= total; - } -} - -double BalanceGoal::evaluate(const GoalContext& context) const -{ - tf::Vector3 center = tf::Vector3(0, 0, 0); - for(size_t i = 0; i < balance_infos.size(); i++) - { - auto& info = balance_infos[i]; - auto& frame = context.getLinkFrame(i); - auto c = info.center; - quat_mul_vec(frame.rot, c, c); - c += frame.pos; - center += c * info.weight; - } - center -= target_; - center -= axis_ * axis_.dot(center); - return center.length2(); -} -} diff --git a/src/bio_ik-kinetic-support/src/ik_base.h b/src/bio_ik-kinetic-support/src/ik_base.h deleted file mode 100644 index 607bd5a..0000000 --- a/src/bio_ik-kinetic-support/src/ik_base.h +++ /dev/null @@ -1,214 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include - -#include "forward_kinematics.h" -#include "problem.h" -#include "utils.h" -#include - -#include - -namespace bio_ik -{ - -struct Random -{ - // std::mt19937 rng; - std::minstd_rand rng; - // std::ranlux24 rng; - // std::knuth_b rng; - // std::default_random_engine rng; - - inline double random() { return std::uniform_real_distribution(0, 1)(rng); } - - inline std::size_t random_index(std::size_t s) { return std::uniform_int_distribution(0, s - 1)(rng); } - - std::normal_distribution normal_distribution; - inline double random_gauss() { return normal_distribution(rng); } - - inline double random(double min, double max) { return random() * (max - min) + min; } - - template inline e& random_element(std::vector& l) { return l[random_index(l.size())]; } - - template inline const e& random_element(const std::vector& l) { return l[random_index(l.size())]; } - - XORShift64 _xorshift; - inline size_t fast_random_index(size_t mod) { return _xorshift() % mod; } - template inline const T& fast_random_element(const std::vector& v) { return v[fast_random_index(v.size())]; } - - static const size_t random_buffer_size = 1024 * 1024 * 8; - - const double* make_random_buffer() - { - static std::vector buf; - buf.resize(random_buffer_size); - for(auto& r : buf) - r = random(); - return buf.data(); - } - const double* random_buffer; - size_t random_buffer_index; - inline double fast_random() - { - double r = random_buffer[random_buffer_index & (random_buffer_size - 1)]; - random_buffer_index++; - return r; - } - - const double* make_random_gauss_buffer() - { - // LOG("make_random_gauss_buffer"); - static std::vector buf; - buf.resize(random_buffer_size); - for(auto& r : buf) - r = random_gauss(); - return buf.data(); - } - const double* random_gauss_buffer; - size_t random_gauss_index; - inline double fast_random_gauss() - { - double r = random_gauss_buffer[random_gauss_index & (random_buffer_size - 1)]; - random_gauss_index++; - return r; - } - inline const double* fast_random_gauss_n(size_t n) - { - size_t i = random_gauss_index; - random_gauss_index += n; - if(random_gauss_index >= random_buffer_size) i = 0, random_gauss_index = n; - return random_gauss_buffer + i; - } - - Random() - : rng(std::random_device()()) - { - random_buffer = make_random_buffer(); - random_buffer_index = _xorshift(); - random_gauss_buffer = make_random_gauss_buffer(); - random_gauss_index = _xorshift(); - } -}; - -struct IKBase : Random -{ - IKParams params; - RobotFK model; - RobotInfo modelInfo; - int thread_index; - Problem problem; - std::vector null_tip_frames; - volatile int canceled; - - virtual void step() = 0; - - virtual const std::vector& getSolution() const = 0; - - virtual void setParams(const IKParams& p) {} - - IKBase(const IKParams& p) - : model(p.robot_model) - , modelInfo(p.robot_model) - , params(p) - { - setParams(p); - } - virtual ~IKBase() {} - - virtual void initialize(const Problem& problem) - { - this->problem = problem; - this->problem.initialize2(); - model.initialize(problem.tip_link_indices); - // active_variables = problem.active_variables; - null_tip_frames.resize(problem.tip_link_indices.size()); - } - - double computeSecondaryFitnessActiveVariables(const double* active_variable_positions) { return problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); } - - double computeSecondaryFitnessAllVariables(const std::vector& variable_positions) { return computeSecondaryFitnessActiveVariables(extractActiveVariables(variable_positions)); } - - double computeFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } - - double computeFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } - - double computeCombinedFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) - { - double ret = 0.0; - ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); - ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); - return ret; - } - - double computeCombinedFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) - { - double ret = 0.0; - ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); - ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); - return ret; - } - - bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.checkSolutionActiveVariables(tip_frames, active_variable_positions); } - - bool checkSolution(const std::vector& variable_positions, const std::vector& tips) { return checkSolutionActiveVariables(tips, extractActiveVariables(variable_positions)); } - - std::vector temp_active_variable_positions; - - double* extractActiveVariables(const std::vector& variable_positions) - { - temp_active_variable_positions.resize(problem.active_variables.size()); - for(size_t i = 0; i < temp_active_variable_positions.size(); i++) - temp_active_variable_positions[i] = variable_positions[problem.active_variables[i]]; - return temp_active_variable_positions.data(); - } - - double computeFitness(const std::vector& variable_positions, const std::vector& tip_frames) { return computeFitnessActiveVariables(tip_frames, extractActiveVariables(variable_positions)); } - - double computeFitness(const std::vector& variable_positions) - { - model.applyConfiguration(variable_positions); - return computeFitness(variable_positions, model.getTipFrames()); - } - - virtual size_t concurrency() const { return 1; } -}; - -typedef IKBase IKSolver; - -typedef Factory IKFactory; -} diff --git a/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp b/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp deleted file mode 100644 index 790483d..0000000 --- a/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp +++ /dev/null @@ -1,257 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -#include "cppoptlib/boundedproblem.h" -#include "cppoptlib/meta.h" -#include "cppoptlib/problem.h" - -namespace bio_ik -{ - -/* -struct IKOptLibProblem : cppoptlib::Problem -{ - IKBase* ik; - std::vector fk_values; - IKOptLibProblem(IKBase* ik) : ik(ik) - { - } - void initialize() - { - // set all variables to initial guess, including inactive ones - fk_values = ik->problem.initial_guess; - } - double value(const TVector& x) - { - // fill in active variables and compute fitness - for(size_t i = 0; i < ik->problem.active_variables.size(); i++) fk_values[ik->problem.active_variables[i]] = x[i]; - return ik->computeFitness(fk_values); - } - bool callback(const cppoptlib::Criteria& state, const TVector& x) - { - // check ik timeout - return ros::WallTime::now().toSec() < ik->problem.timeout; - } -}; -*/ - -// problem description for cppoptlib -struct IKOptLibProblem : cppoptlib::BoundedProblem -{ - IKBase* ik; - std::vector fk_values; - IKOptLibProblem(IKBase* ik) - : cppoptlib::BoundedProblem(TVector(ik->problem.active_variables.size()), TVector(ik->problem.active_variables.size())) - , ik(ik) - { - // init bounds - for(size_t i = 0; i < ik->problem.active_variables.size(); i++) - { - m_lowerBound[i] = ik->modelInfo.getMin(ik->problem.active_variables[i]); - m_upperBound[i] = ik->modelInfo.getMax(ik->problem.active_variables[i]); - // m_lowerBound[i] = fmax(m_lowerBound[i], -100); - // m_upperBound[i] = fmin(m_upperBound[i], 100); - } - } - void initialize() - { - // set all variables to initial guess, including inactive ones - fk_values = ik->problem.initial_guess; - } - double value(const TVector& x) - { - // fill in active variables and compute fitness - for(size_t i = 0; i < ik->problem.active_variables.size(); i++) - fk_values[ik->problem.active_variables[i]] = x[i]; - // for(size_t i = 0; i < ik->active_variables.size(); i++) LOG(i, x[i]); LOG(""); - // for(size_t i = 0; i < ik->active_variables.size(); i++) std::cerr << ((void*)*(uint64_t*)&x[i]) << " "; std::cerr << std::endl; - // size_t h = 0; for(size_t i = 0; i < ik->active_variables.size(); i++) h ^= (std::hash()(x[i]) << i); LOG((void*)h); - return ik->computeFitness(fk_values); - } - bool callback(const cppoptlib::Criteria& state, const TVector& x) - { - // check ik timeout - return ros::WallTime::now().toSec() < ik->problem.timeout; - } -}; - -// ik solver using cppoptlib -template struct IKOptLib : IKBase -{ - // current solution - std::vector solution, best_solution, temp; - - // cppoptlib solver - std::shared_ptr solver; - - // cppoptlib problem description - IKOptLibProblem f; - - // reset flag - bool reset; - - // stop criteria - cppoptlib::Criteria crit; - - IKOptLib(const IKParams& p) - : IKBase(p) - , f(this) - { - } - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - - // set initial guess - solution = problem.initial_guess; - - // randomize if more than 1 thread - reset = false; - if(thread_index > 0) reset = true; - - // init best solution - best_solution = solution; - - // initialize cppoptlib problem description - f = IKOptLibProblem(this); - f.initialize(); - - // init stop criteria (timeout will be handled explicitly) - crit = cppoptlib::Criteria::defaults(); - // crit.iterations = SIZE_MAX; - crit.gradNorm = 1e-10; - // p.node_handle.param("optlib_stop", crit.gradNorm, crit.gradNorm); - - if(!solver) solver = std::make_shared(); - } - - const std::vector& getSolution() const { return best_solution; } - - void step() - { - // set stop criteria - solver->setStopCriteria(crit); - - // random reset if stuck (and if random resets are enabled) - if(reset) - { - // LOG("RESET"); - reset = false; - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - - // set to current positions - temp = solution; - typename SOLVER::TVector x(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - x[i] = temp[problem.active_variables[i]]; - - // solve - solver->minimize(f, x); - - // get results - for(size_t i = 0; i < problem.active_variables.size(); i++) - temp[problem.active_variables[i]] = x[i]; - - // update solution - if(computeFitness(temp) < computeFitness(solution)) - { - solution = temp; - } - else - { - if(reset_if_stuck) reset = true; - } - - // update best solution - if(computeFitness(solution) < computeFitness(best_solution)) - { - best_solution = solution; - } - } - - size_t concurrency() const { return threads; } -}; -} - -static std::string mkoptname(std::string name, int reset, int threads) -{ - name = "optlib_" + name; - if(reset) name += "_r"; - if(threads > 1) name += "_" + std::to_string(threads); - return name; -} - -#define IKCPPOPTX(n, t, reset, threads) static bio_ik::IKFactory::Class, reset, threads>> ik_optlib_##t##reset##threads(mkoptname(n, reset, threads)); - -//#define IKCPPOPT(n, t) static bio_ik::IKFactory::Class>> ik_optlib_##t(n) - -#define IKCPPOPT(n, t) \ - IKCPPOPTX(n, t, 0, 1) \ - IKCPPOPTX(n, t, 0, 2) \ - IKCPPOPTX(n, t, 0, 4) \ - IKCPPOPTX(n, t, 1, 1) \ - IKCPPOPTX(n, t, 1, 2) \ - IKCPPOPTX(n, t, 1, 4) - -#include "cppoptlib/solver/bfgssolver.h" -IKCPPOPT("bfgs", BfgsSolver); - -//#include "cppoptlib/solver/cmaesbsolver.h" -// IKCPPOPT("cmaesb", CMAesBSolver); - -//#include "cppoptlib/solver/cmaessolver.h" -// IKCPPOPT("cmaes", CMAesSolver); - -#include "cppoptlib/solver/conjugatedgradientdescentsolver.h" -IKCPPOPT("cgd", ConjugatedGradientDescentSolver); - -#include "cppoptlib/solver/gradientdescentsolver.h" -IKCPPOPT("gd", GradientDescentSolver); - -#include "cppoptlib/solver/lbfgsbsolver.h" -IKCPPOPT("lbfgsb", LbfgsbSolver); - -#include "cppoptlib/solver/lbfgssolver.h" -IKCPPOPT("lbfgs", LbfgsSolver); - -#include "cppoptlib/solver/neldermeadsolver.h" -IKCPPOPT("nm", NelderMeadSolver); - -#include "cppoptlib/solver/newtondescentsolver.h" -IKCPPOPT("nd", NewtonDescentSolver); diff --git a/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp b/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp deleted file mode 100644 index 5435e10..0000000 --- a/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp +++ /dev/null @@ -1,561 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -namespace bio_ik -{ - -struct IKEvolution1 : IKBase -{ - struct Individual - { - std::vector genes; - std::vector gradients; - double extinction; - double fitness; - }; - - class HeuristicErrorTree - { - size_t variable_count, tip_count; - std::vector table; - std::vector chain_lengths; - std::vector> chain_lengths_2; - - public: - HeuristicErrorTree() {} - HeuristicErrorTree(moveit::core::RobotModelConstPtr robot_model, const std::vector& tip_names) - { - tip_count = tip_names.size(); - variable_count = robot_model->getVariableCount(); - table.resize(tip_count * variable_count); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - auto& tip_name = tip_names[tip_index]; - for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) - { - auto* joint_model = link_model->getParentJointModel(); - size_t v1 = joint_model->getFirstVariableIndex(); - size_t vn = joint_model->getVariableCount(); - for(size_t variable_index = v1; variable_index < v1 + vn; variable_index++) - table[variable_index * tip_count + tip_index] = 1; - } - } - for(size_t variable_index = 0; variable_index < variable_count; variable_index++) - { - double sum = 0; - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - sum += table[variable_index * tip_count + tip_index]; - if(sum > 0) - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - table[variable_index * tip_count + tip_index] /= sum; - } - - chain_lengths.resize(tip_count); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - auto& tip_name = tip_names[tip_index]; - double chain_length = 0; - for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) - { - chain_length += Frame(link_model->getJointOriginTransform()).pos.length(); - } - chain_lengths[tip_index] = chain_length; - } - - chain_lengths_2.resize(tip_count); - for(size_t tip_index = 0; tip_index < tip_count; tip_index++) - { - auto& tip_name = tip_names[tip_index]; - double chain_length = 0; - chain_lengths_2[tip_index].resize(variable_count, 0.0); - for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel()) - { - - auto* joint_model = link_model->getParentJointModel(); - int vmin = joint_model->getFirstVariableIndex(); - int vmax = vmin + joint_model->getVariableCount(); - for(int vi = vmin; vi < vmax; vi++) - chain_lengths_2[tip_index][vi] = chain_length; - chain_length += Frame(link_model->getJointOriginTransform()).pos.length(); - } - } - } - inline double getInfluence(size_t variable_index, size_t tip_index) const { return table[variable_index * tip_count + tip_index]; } - inline double getChainLength(size_t tip_index) const { return chain_lengths[tip_index]; } - inline double getJointVariableChainLength(size_t tip_index, size_t variable_index) const { return chain_lengths_2[tip_index][variable_index]; } - }; - - HeuristicErrorTree heuristicErrorTree; - std::vector solution; - std::vector population; - int populationSize, eliteCount; - std::vector tempPool; - std::vector tempOffspring; - std::vector initialGuess; - - bool opt_no_wipeout; - - bool linear_fitness; - - void setParams(const IKParams& p) - { - opt_no_wipeout = p.opt_no_wipeout; - populationSize = p.population_size; - eliteCount = p.elite_count; - linear_fitness = p.linear_fitness; - } - - bool in_final_adjustment_loop; - - template inline t select(const std::vector& v) - { - // FNPROFILER(); - linear_int_distribution d(v.size()); - size_t index = d(rng); - return v[index]; - } - - inline double clip(double v, size_t i) { return modelInfo.clip(v, i); } - - inline double getMutationStrength(size_t i, const Individual& parentA, const Individual& parentB) - { - double extinction = 0.5 * (parentA.extinction + parentB.extinction); - double span = modelInfo.getSpan(i); - return span * extinction; - } - - double computeAngularScale(size_t tip_index, const Frame& tip_frame) - { - double angular_scale = sqrt(heuristicErrorTree.getChainLength(tip_index) * tip_frame.pos.length()) / M_PI; - return angular_scale; - // return 1; - /*double angular_scale = sqrt(heuristicErrorTree.getChainLength(tip_index) * tip_frame.pos.length()) / M_PI; - //double angular_scale = heuristicErrorTree.getChainLength(tip_index) * (1.0 / M_PI); - if(opt_angular_scale_full_circle) angular_scale *= 0.5; - return angular_scale;*/ - } - - double getHeuristicError(size_t variable_index, bool balanced) - { - // return 1; - - double heuristic_error = 0; - // for(int tip_index = 0; tip_index < tipObjectives.size(); tip_index++) - for(int tip_index = 0; tip_index < problem.goals.size(); tip_index++) - { - double influence = heuristicErrorTree.getInfluence(variable_index, tip_index); - if(influence == 0) continue; - - // const auto& ta = tipObjectives[tip_index]; - const auto& ta = problem.goals[tip_index].frame; - const auto& tb = model.getTipFrame(tip_index); - - double length = heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index); - - // LOG_ALWAYS("a", heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index)); - - // double length = model.getJointVariableFrame(variable_index).pos.distance(model.getTipFrame(tip_index).pos); if(length <= 0.000000001) length = 0; - - // LOG_ALWAYS("b", length); - - if(modelInfo.isPrismatic(variable_index)) - { - // heuristic_error += ta.pos.distance(tb.pos) * influence; - // if(length) heuristic_error += ta.rot.angle(tb.rot) * length * influence; - - if(length) - { - heuristic_error += ta.pos.distance(tb.pos) * influence * 0.5; - heuristic_error += ta.rot.angle(tb.rot) * length * influence * 0.5; - } - else - { - heuristic_error += ta.pos.distance(tb.pos) * influence; - } - } - - if(modelInfo.isRevolute(variable_index)) - { - // if(length) heuristic_error += ta.pos.distance(tb.pos) / length * influence; - // heuristic_error += ta.rot.angle(tb.rot) * influence; - - if(length) - { - heuristic_error += ta.pos.distance(tb.pos) / length * influence * 0.5; - heuristic_error += ta.rot.angle(tb.rot) * influence * 0.5; - } - else - { - heuristic_error += ta.rot.angle(tb.rot) * influence; - } - - // double d = 0.0; - // if(length) d = std::max(d, ta.pos.distance(tb.pos) / length); - // d = std::max(d, ta.rot.angle(tb.rot)); - // heuristic_error += d * influence; - } - } - // heuristic_error *= 0.5; - // LOG_ALWAYS(heuristic_error); - return heuristic_error; - } - - bool in_adjustment_2, in_get_solution_fitness; - - void reroll(Individual& offspring) - { - FNPROFILER(); - // for(size_t i = 0; i < offspring.genes.size(); i++) - for(auto i : problem.active_variables) - { - offspring.genes[i] = random(modelInfo.getMin(i), modelInfo.getMax(i)); - - offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random(0.0, 0.1)); - - offspring.gradients[i] = 0; - } - offspring.fitness = computeFitness(offspring.genes, false); - } - - double computeFitness(const std::vector& genes, bool balanced) - { - if(linear_fitness) - { - model.applyConfiguration(genes); - double fitness_sum = 0.0; - for(size_t goal_index = 0; goal_index < problem.goals.size(); goal_index++) - { - const auto& ta = problem.goals[goal_index].frame; - const auto& tb = model.getTipFrame(problem.goals[goal_index].tip_index); - - double tdist = ta.pos.distance(tb.pos) / computeAngularScale(problem.goals[goal_index].tip_index, ta); - double rdist = ta.rot.angle(tb.rot); - - fitness_sum += mix(tdist, rdist, (balanced || in_final_adjustment_loop) ? 0.5 : random()); - } - return fitness_sum; - } - else - { - return IKBase::computeFitness(genes); - } - } - - bool checkWipeout() - { - FNPROFILER(); - auto& genes = population[0].genes; - // for(size_t i = 0; i < genes.size(); i++) - for(auto i : problem.active_variables) - { - double v0 = genes[i]; - double fitness = computeFitness(genes, true); - double heuristicError = getHeuristicError(i, true); - // double heuristicError = 0.001; - genes[i] = modelInfo.clip(v0 + random(0, heuristicError), i); - double incFitness = computeFitness(genes, true); - genes[i] = modelInfo.clip(v0 - random(0, heuristicError), i); - double decFitness = computeFitness(genes, true); - genes[i] = v0; - if(incFitness < fitness || decFitness < fitness) - { - // LOG("no wipeout"); - return false; - } - } - // LOG("wipeout 1"); - return true; - } - - void computeExtinctions() - { - double min = population.front().fitness; - double max = population.back().fitness; - for(size_t i = 0; i < populationSize; i++) - { - double grading = (double)i / (double)(populationSize - 1); - population[i].extinction = (population[i].fitness + min * (grading - 1)) / max; - } - } - - bool tryUpdateSolution() - { - FNPROFILER(); - double solutionFitness = computeFitness(solution, true); - double candidateFitness = computeFitness(population[0].genes, true); - // LOG_VAR(solutionFitness); - // LOG_VAR(candidateFitness); - if(candidateFitness < solutionFitness) - { - solution = population[0].genes; - // solution = initialGuess; - // for(auto i : problem.active_variables) - // solution[i] = population[0].genes[i]; - return true; - } - return false; - } - - double getMutationProbability(const Individual& parentA, const Individual& parentB) - { - double extinction = 0.5 * (parentA.extinction + parentB.extinction); - double inverse = 1.0 / parentA.genes.size(); - return extinction * (1.0 - inverse) + inverse; - } - - void sortByFitness() - { - FNPROFILER(); - sort(population.begin(), population.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); - } - - double bounce(double v, int i) - { - double c = clip(v, i); - v = c - (v - c) * 2; - // v = c + c - v; - v = clip(v, i); - return v; - } - - void reproduce(Individual& offspring, const Individual& parentA, const Individual& parentB, const Individual& prototype) - { - FNPROFILER(); - for(size_t i = 0; i < offspring.genes.size(); i++) - // for(auto i : problem.active_variables) - { - offspring.genes[i] = mix(parentA.genes[i], parentB.genes[i], random()); - offspring.genes[i] += parentA.gradients[i] * random(); - offspring.genes[i] += parentB.gradients[i] * random(); - - double storage = offspring.genes[i]; - - if(random() < getMutationProbability(parentA, parentB)) offspring.genes[i] += random(-1, 1) * getMutationStrength(i, parentA, parentB); - // offspring.genes[i] += normal_random() * getMutationStrength(i, parentA, parentB); - - offspring.genes[i] += mix(random() * (0.5 * (parentA.genes[i] + parentB.genes[i]) - offspring.genes[i]), random() * (prototype.genes[i] - offspring.genes[i]), random()); - - // offspring.genes[i] = clip(offspring.genes[i], i); - - // offspring.genes[i] += fabs(offspring.genes[i] - storage) * offspring.genes[i] - (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5; - - // offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random() * 0.1 * fabs(offspring.genes[i] - storage) / modelInfo.getSpan(i)); - - offspring.genes[i] = clip(offspring.genes[i], i); - - // offspring.genes[i] = bounce(offspring.genes[i], i); - - offspring.gradients[i] = random() * offspring.gradients[i] + offspring.genes[i] - storage; - } - - offspring.fitness = computeFitness(offspring.genes, false); - } - - void exploit(Individual& individual) - { - FNPROFILER(); - - double fitness_sum = 0; - - // model.incrementalBegin(individual.genes); - - for(auto i : problem.active_variables) - { - double fitness = computeFitness(individual.genes, true); - - double heuristicError = getHeuristicError(i, true); - double v_0 = individual.genes[i]; - - double v_inc = clip(v_0 + random(0, heuristicError), i); - double v_dec = clip(v_0 - random(0, heuristicError), i); - - individual.genes[i] = v_inc; - double inc_fitness = computeFitness(individual.genes, true); - individual.genes[i] = v_dec; - double dec_fitness = computeFitness(individual.genes, true); - - if(inc_fitness < fitness && inc_fitness <= dec_fitness) - { - individual.genes[i] = v_inc; - individual.gradients[i] = v_0 * random() + v_inc - v_0; - fitness_sum += inc_fitness; - } - else if(dec_fitness < fitness && dec_fitness <= inc_fitness) - { - individual.genes[i] = v_dec; - individual.gradients[i] = v_0 * random() + v_dec - v_0; - fitness_sum += dec_fitness; - } - else - { - individual.genes[i] = v_0; - fitness_sum += fitness; - } - } - - // model.incrementalEnd(); - - individual.fitness = fitness_sum / individual.genes.size(); - } - - IKEvolution1(const IKParams& p) - : IKBase(p) - , populationSize(12) - , eliteCount(4) - , in_final_adjustment_loop(false) - , in_adjustment_2(false) - , in_get_solution_fitness(false) - { - setParams(p); - } - - void init() - { - initialGuess = problem.initial_guess; - solution = initialGuess; - - population.resize(populationSize); - - { - auto& p = population[0]; - p.genes = solution; - p.gradients.clear(); - p.gradients.resize(p.genes.size(), 0); - p.fitness = computeFitness(p.genes, false); - } - - for(int i = 1; i < populationSize; i++) - { - auto& p = population[i]; - p.genes = solution; - p.gradients.clear(); - p.gradients.resize(p.genes.size(), 0); - reroll(p); - } - - sortByFitness(); - computeExtinctions(); - } - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - - std::vector tips; - for(auto tip_link_index : problem.tip_link_indices) - tips.push_back(params.robot_model->getLinkModelNames()[tip_link_index]); - heuristicErrorTree = HeuristicErrorTree(params.robot_model, tips); - - init(); - } - - const std::vector& getSolution() const { return solution; } - - double getSolutionFitness() - { - in_get_solution_fitness = true; - double f = computeFitness(solution, true); - in_get_solution_fitness = false; - return f; - } - - const std::vector& getSolutionTipFrames() - { - model.applyConfiguration(solution); - return model.getTipFrames(); - } - - bool evolve() - { - FNPROFILER(); - - auto& offspring = tempOffspring; - offspring = population; - - for(size_t i = 0; i < eliteCount; i++) - { - offspring[i] = population[i]; - exploit(offspring[i]); - } - - auto& pool = tempPool; - pool.resize(populationSize); - iota(pool.begin(), pool.end(), &population[0]); - - for(size_t i = eliteCount; i < populationSize; i++) - { - if(pool.size() > 0) - { - auto& parentA = *select(pool); - auto& parentB = *select(pool); - auto& prototype = *select(pool); - reproduce(offspring[i], parentA, parentB, prototype); - if(offspring[i].fitness < parentA.fitness) pool.erase(remove(pool.begin(), pool.end(), &parentA), pool.end()); - if(offspring[i].fitness < parentB.fitness) pool.erase(remove(pool.begin(), pool.end(), &parentB), pool.end()); - } - else - { - reroll(offspring[i]); - } - } - - population = offspring; - - sortByFitness(); - - computeExtinctions(); - - if(tryUpdateSolution()) return true; - if(opt_no_wipeout) return false; - if(!checkWipeout()) return false; - - init(); - - return tryUpdateSolution(); - } - - void step() - { - in_adjustment_2 = false; - evolve(); - } - - virtual size_t concurrency() const { return 4; } -}; - -static IKFactory::Class cIKEvolution1("bio1"); - -} diff --git a/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp b/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp deleted file mode 100644 index 2a4bb8a..0000000 --- a/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp +++ /dev/null @@ -1,659 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -#ifdef ENABLE_CPP_OPTLIB -#include "cppoptlib/solver/lbfgssolver.h" -#endif - -namespace bio_ik -{ - -// fast evolutionary inverse kinematics -template struct IKEvolution2 : IKBase -{ - struct Individual - { - aligned_vector genes; - aligned_vector gradients; - double fitness; - }; - - struct Species - { - std::vector individuals; - double fitness; - bool improved; - }; - - std::vector initial_guess; - std::vector solution; - std::vector temp_joint_variables; - double solution_fitness; - std::vector species; - std::vector children; - std::vector> phenotypes, phenotypes2, phenotypes3; - std::vector child_indices; - std::vector genotypes; - std::vector phenotype; - std::vector quaternion_genes; - aligned_vector genes_min, genes_max, genes_span; - aligned_vector gradient, temp; - - IKEvolution2(const IKParams& p) - : IKBase(p) - { - } - -#ifdef ENABLE_CPP_OPTLIB - struct OptlibProblem : cppoptlib::Problem - { - IKEvolution2* ik; - OptlibProblem(IKEvolution2* ik) - : ik(ik) - { - } - double value(const TVector& x) - { - const double* genes = x.data(); - ik->model.computeApproximateMutations(1, &genes, ik->phenotypes); - return ik->computeCombinedFitnessActiveVariables(ik->phenotypes[0], genes); - } - }; - typedef cppoptlib::LbfgsSolver OptlibSolver; - std::shared_ptr optlib_solver; - std::shared_ptr optlib_problem; - typename OptlibSolver::TVector optlib_vector; -#endif - - void genesToJointVariables(const Individual& individual, std::vector& variables) - { - auto& genes = individual.genes; - variables.resize(params.robot_model->getVariableCount()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - variables[problem.active_variables[i]] = genes[i]; - } - - const std::vector& getSolution() const { return solution; } - - void initialize(const Problem& problem) - { - BLOCKPROFILER("initialization"); - - IKBase::initialize(problem); - - // init list of quaternion joint genes to be normalized during each mutation - quaternion_genes.clear(); - for(size_t igene = 0; igene < problem.active_variables.size(); igene++) - { - size_t ivar = problem.active_variables[igene]; - auto* joint_model = params.robot_model->getJointOfVariable(ivar); - if(joint_model->getFirstVariableIndex() + 3 != ivar) continue; - if(joint_model->getType() != moveit::core::JointModel::FLOATING) continue; - quaternion_genes.push_back(igene); - } - - // set solution to initial guess - initial_guess = problem.initial_guess; - solution = initial_guess; - solution_fitness = computeFitness(solution); - - // init temporary buffer with positions of inactive joints - temp_joint_variables = initial_guess; - - // params - size_t population_size = 2; - size_t child_count = 16; - - // initialize population on current island - species.resize(2); - for(auto& s : species) - { - // create individuals - s.individuals.resize(population_size); - - // initialize first individual - { - auto& v = s.individuals[0]; - - // init genes - v.genes.resize(problem.active_variables.size()); - // if(thread_index == 0) // on first island? - // if(thread_index % 2 == 0) // on every second island... - if(1) - { - // set to initial_guess - for(size_t i = 0; i < v.genes.size(); i++) - v.genes[i] = initial_guess[problem.active_variables[i]]; - } - else - { - // initialize populations on other islands randomly - for(size_t i = 0; i < v.genes.size(); i++) - v.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i])); - } - - // set gradients to zero - v.gradients.clear(); - v.gradients.resize(problem.active_variables.size(), 0); - } - - // clone first individual - for(size_t i = 1; i < s.individuals.size(); i++) - { - s.individuals[i].genes = s.individuals[0].genes; - s.individuals[i].gradients = s.individuals[0].gradients; - } - } - - // space for child population - children.resize(population_size + child_count); - for(auto& child : children) - { - child.genes.resize(problem.active_variables.size()); - child.gradients.resize(problem.active_variables.size()); - } - - // init gene infos - // if(genes_min.empty()) - { - genes_min.resize(problem.active_variables.size()); - genes_max.resize(problem.active_variables.size()); - genes_span.resize(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - { - genes_min[i] = modelInfo.getClipMin(problem.active_variables[i]); - genes_max[i] = modelInfo.getClipMax(problem.active_variables[i]); - genes_span[i] = modelInfo.getSpan(problem.active_variables[i]); - } - } - - /* - // init chain mutation masks - chain_mutation_masks.resize(chain_mutation_mask_count); - for(auto& chain_mutation_mask : chain_mutation_masks) - { - temp_mutation_chain.clear(); - if(problem.tip_link_indices.size() > 1) - { - for(auto* chain_link = params.robot_model->getLinkModel(random_element(problem.tip_link_indices)); chain_link; chain_link = chain_link->getParentLinkModel()) - temp_mutation_chain.push_back(chain_link); - temp_mutation_chain.resize(random_index(temp_mutation_chain.size()) + 1); - } - - temp_chain_mutation_var_mask.resize(params.robot_model->getVariableCount()); - for(auto& m : temp_chain_mutation_var_mask) m = 0; - for(auto* chain_link : temp_mutation_chain) - { - auto* chain_joint = chain_link->getParentJointModel(); - for(size_t ivar = chain_joint->getFirstVariableIndex(); ivar < chain_joint->getFirstVariableIndex() + chain_joint->getVariableCount(); ivar++) - temp_chain_mutation_var_mask[ivar] = 1; - } - - chain_mutation_mask.resize(problem.active_variables.size()); - for(size_t igene = 0; igene < problem.active_variables.size(); igene++) - chain_mutation_mask[igene] = temp_chain_mutation_var_mask[problem.active_variables[igene]]; - } - */ - } - - /* - const size_t chain_mutation_mask_count = 256; - std::vector> chain_mutation_masks; - std::vector temp_mutation_chain; - std::vector temp_chain_mutation_var_mask; - */ - - // aligned_vector rmask; - - // create offspring and mutate - __attribute__((hot)) __attribute__((noinline)) - //__attribute__((target_clones("avx2", "avx", "sse2", "default"))) - //__attribute__((target("avx"))) - void - reproduce(const std::vector& population) - { - const auto __attribute__((aligned(32)))* __restrict__ genes_span = this->genes_span.data(); - const auto __attribute__((aligned(32)))* __restrict__ genes_min = this->genes_min.data(); - const auto __attribute__((aligned(32)))* __restrict__ genes_max = this->genes_max.data(); - - auto gene_count = children[0].genes.size(); - - size_t s = (children.size() - population.size()) * gene_count + children.size() * 4 + 4; - - auto* __restrict__ rr = fast_random_gauss_n(s); - rr = (const double*)(((size_t)rr + 3) / 4 * 4); - - /*rmask.resize(s); - for(auto& m : rmask) m = fast_random() < 0.1 ? 1.0 : 0.0; - double* dm = rmask.data();*/ - - for(size_t child_index = population.size(); child_index < children.size(); child_index++) - { - double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23)); - auto& parent = population[0]; - auto& parent2 = population[1]; - double fmix = (child_index % 2 == 0) * 0.2; - double gradient_factor = child_index % 3; - - auto __attribute__((aligned(32)))* __restrict__ parent_genes = parent.genes.data(); - auto __attribute__((aligned(32)))* __restrict__ parent_gradients = parent.gradients.data(); - - auto __attribute__((aligned(32)))* __restrict__ parent2_genes = parent2.genes.data(); - auto __attribute__((aligned(32)))* __restrict__ parent2_gradients = parent2.gradients.data(); - - auto& child = children[child_index]; - - auto __attribute__((aligned(32)))* __restrict__ child_genes = child.genes.data(); - auto __attribute__((aligned(32)))* __restrict__ child_gradients = child.gradients.data(); - -#pragma unroll -#pragma omp simd aligned(genes_span : 32), aligned(genes_min : 32), aligned(genes_max : 32), aligned(parent_genes : 32), aligned(parent_gradients : 32), aligned(parent2_genes : 32), aligned(parent2_gradients : 32), aligned(child_genes : 32), aligned(child_gradients : 32) aligned(rr : 32) - for(size_t gene_index = 0; gene_index < gene_count; gene_index++) - { - // double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23)); - - double r = rr[gene_index]; - // r *= dm[gene_index]; - double f = mutation_rate * genes_span[gene_index]; - double gene = parent_genes[gene_index]; - double parent_gene = gene; - gene += r * f; - double parent_gradient = mix(parent_gradients[gene_index], parent2_gradients[gene_index], fmix); - double gradient = parent_gradient * gradient_factor; - gene += gradient; - gene = clamp(gene, genes_min[gene_index], genes_max[gene_index]); - child_genes[gene_index] = gene; - child_gradients[gene_index] = mix(parent_gradient, gene - parent_gene, 0.3); - } - rr += (gene_count + 3) / 4 * 4; - // dm += (gene_count + 3) / 4 * 4; - - /*if(problem.tip_link_indices.size() > 1) - { - if(fast_random() < 0.5) - { - auto& mask = chain_mutation_masks[fast_random_index(chain_mutation_mask_count)]; - for(size_t gene_index = 0; gene_index < gene_count; gene_index++) - { - if(!mask[gene_index]) - { - child_genes[gene_index] = parent_genes[gene_index]; - child_gradients[gene_index] = parent_gradients[gene_index]; - } - } - } - }*/ - - for(auto quaternion_gene_index : quaternion_genes) - { - auto& qpos = (*(Quaternion*)&(children[child_index].genes[quaternion_gene_index])); - normalizeFast(qpos); - } - } - } - - void step() - { - FNPROFILER(); - - for(size_t ispecies = 0; ispecies < species.size(); ispecies++) - { - auto& species = this->species[ispecies]; - auto& population = species.individuals; - - { - BLOCKPROFILER("evolution"); - - // initialize forward kinematics approximator - genesToJointVariables(species.individuals[0], temp_joint_variables); - { - BLOCKPROFILER("fk"); - model.applyConfiguration(temp_joint_variables); - model.initializeMutationApproximator(problem.active_variables); - } - - // run evolution for a few generations - size_t generation_count = 16; - if(memetic) generation_count = 8; - for(size_t generation = 0; generation < generation_count; generation++) - { - // BLOCKPROFILER("evolution"); - - if(canceled) break; - - // reproduction - { - BLOCKPROFILER("reproduction"); - reproduce(population); - } - - size_t child_count = children.size(); - - // pre-selection by secondary objectives - if(problem.secondary_goals.size()) - { - BLOCKPROFILER("pre-selection"); - child_count = random_index(children.size() - population.size() - 1) + 1 + population.size(); - for(size_t child_index = population.size(); child_index < children.size(); child_index++) - { - children[child_index].fitness = computeSecondaryFitnessActiveVariables(children[child_index].genes.data()); - } - { - BLOCKPROFILER("pre-selection sort"); - std::sort(children.begin() + population.size(), children.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; }); - } - } - - // keep parents - { - BLOCKPROFILER("keep alive"); - for(size_t i = 0; i < population.size(); i++) - { - children[i].genes = population[i].genes; - children[i].gradients = population[i].gradients; - } - } - - // genotype-phenotype mapping - { - BLOCKPROFILER("phenotype"); - size_t gene_count = children[0].genes.size(); - genotypes.resize(child_count); - for(size_t i = 0; i < child_count; i++) - genotypes[i] = children[i].genes.data(); - model.computeApproximateMutations(child_count, genotypes.data(), phenotypes); - } - - // fitness - { - BLOCKPROFILER("fitness"); - for(size_t child_index = 0; child_index < child_count; child_index++) - { - children[child_index].fitness = computeFitnessActiveVariables(phenotypes[child_index], genotypes[child_index]); - } - } - - // selection - { - BLOCKPROFILER("selection"); - child_indices.resize(child_count); - for(size_t i = 0; i < child_count; i++) - child_indices[i] = i; - for(size_t i = 0; i < population.size(); i++) - { - size_t jmin = i; - double fmin = children[child_indices[i]].fitness; - for(size_t j = i + 1; j < child_count; j++) - { - double f = children[child_indices[j]].fitness; - if(f < fmin) jmin = j, fmin = f; - } - std::swap(child_indices[i], child_indices[jmin]); - } - for(size_t i = 0; i < population.size(); i++) - { - std::swap(population[i].genes, children[child_indices[i]].genes); - std::swap(population[i].gradients, children[child_indices[i]].gradients); - } - } - } - } - - // memetic optimization - { - BLOCKPROFILER("memetics"); - - if(memetic == 'q' || memetic == 'l') - { - - // init - auto& individual = population[0]; - gradient.resize(problem.active_variables.size()); - if(genotypes.empty()) genotypes.emplace_back(); - phenotypes2.resize(1); - phenotypes3.resize(1); - - // differentiation step size - double dp = 0.0000001; - if(fast_random() < 0.5) dp = -dp; - - for(size_t generation = 0; generation < 8; generation++) - // for(size_t generation = 0; generation < 32; generation++) - { - - if(canceled) break; - - // compute gradient - temp = individual.genes; - genotypes[0] = temp.data(); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); - double f2p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); - double fa = f2p + computeSecondaryFitnessActiveVariables(genotypes[0]); - for(size_t i = 0; i < problem.active_variables.size(); i++) - { - double* pp = &(genotypes[0][i]); - genotypes[0][i] = individual.genes[i] + dp; - model.computeApproximateMutation1(problem.active_variables[i], +dp, phenotypes2[0], phenotypes3[0]); - double fb = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - genotypes[0][i] = individual.genes[i]; - double d = fb - fa; - gradient[i] = d; - } - - // normalize gradient - double sum = dp * dp; - for(size_t i = 0; i < problem.active_variables.size(); i++) - sum += fabs(gradient[i]); - double f = 1.0 / sum * dp; - for(size_t i = 0; i < problem.active_variables.size(); i++) - gradient[i] *= f; - - // sample support points for line search - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = individual.genes[i] - gradient[i]; - model.computeApproximateMutations(1, genotypes.data(), phenotypes3); - double f1 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - - double f2 = fa; - - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = individual.genes[i] + gradient[i]; - model.computeApproximateMutations(1, genotypes.data(), phenotypes3); - double f3 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]); - - // quadratic step size - if(memetic == 'q') - { - - // compute step size - double v1 = (f2 - f1); // f / j - double v2 = (f3 - f2); // f / j - double v = (v1 + v2) * 0.5; // f / j - double a = (v1 - v2); // f / j^2 - double step_size = v / a; // (f / j) / (f / j^2) = f / j / f * j * j = j - - // double v1 = (f2 - f1) / dp; - // double v2 = (f3 - f2) / dp; - // double v = (v1 + v2) * 0.5; - // double a = (v2 - v1) / dp; - // // v * x + a * x * x = 0; - // // v = - a * x - // // - v / a = x - // // x = -v / a; - // double step_size = -v / a / dp; - - // for(double f : { 1.0, 0.5, 0.25 }) - { - - double f = 1.0; - - // move by step size along gradient and compute fitness - for(size_t i = 0; i < problem.active_variables.size(); i++) - genotypes[0][i] = modelInfo.clip(individual.genes[i] + gradient[i] * step_size * f, problem.active_variables[i]); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); - double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); - - // accept new position if better - if(f4p < f2p) - { - individual.genes = temp; - continue; - } - else - { - break; - } - } - - // break; - } - - // linear step size - if(memetic == 'l') - { - - // compute step size - double cost_diff = (f3 - f1) * 0.5; // f / j - double step_size = f2 / cost_diff; // f / (f / j) = j - - // move by step size along gradient and compute fitness - for(size_t i = 0; i < problem.active_variables.size(); i++) - temp[i] = modelInfo.clip(individual.genes[i] - gradient[i] * step_size, problem.active_variables[i]); - model.computeApproximateMutations(1, genotypes.data(), phenotypes2); - double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]); - - // accept new position if better - if(f4p < f2p) - { - individual.genes = temp; - continue; - } - else - { - break; - } - } - } - } - -#ifdef ENABLE_CPP_OPTLIB - // cppoptlib::LbfgsSolver memetic test - if(memetic == 'o') - { - auto& individual = population[0]; - - // create cppoptlib solver and cppoptlib problem, if not yet existing - if(!optlib_solver) - { - optlib_solver = std::make_shared(); - cppoptlib::Criteria crit; - crit.iterations = 4; - optlib_solver->setStopCriteria(crit); - optlib_problem = std::make_shared(this); - } - - // init starting point - optlib_vector.resize(problem.active_variables.size()); - for(size_t i = 0; i < problem.active_variables.size(); i++) - optlib_vector[i] = individual.genes[i]; - - // solve - optlib_solver->minimize(*optlib_problem, optlib_vector); - - // get result - for(size_t i = 0; i < problem.active_variables.size(); i++) - individual.genes[i] = modelInfo.clip(optlib_vector[i], problem.active_variables[i]); - } -#endif - } - } - - { - BLOCKPROFILER("species"); - - // compute species fitness - for(auto& species : this->species) - { - genesToJointVariables(species.individuals[0], temp_joint_variables); - double fitness = computeFitness(temp_joint_variables); - species.improved = (fitness != species.fitness); - species.fitness = fitness; - } - - // sort species by fitness - std::sort(species.begin(), species.end(), [](const Species& a, const Species& b) { return a.fitness < b.fitness; }); - - // wipeouts - for(size_t species_index = 1; species_index < species.size(); species_index++) - { - if(fast_random() < 0.1 || !species[species_index].improved) - // if(fast_random() < 0.05 || !species[species_index].improved) - { - { - auto& individual = species[species_index].individuals[0]; - - for(size_t i = 0; i < individual.genes.size(); i++) - individual.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i])); - - for(auto& v : individual.gradients) - v = 0; - } - for(size_t i = 0; i < species[species_index].individuals.size(); i++) - species[species_index].individuals[i] = species[species_index].individuals[0]; - } - } - - // update solution - if(species[0].fitness < solution_fitness) - { - genesToJointVariables(species[0].individuals[0], solution); - solution_fitness = species[0].fitness; - } - } - } - - // number of islands - virtual size_t concurrency() const { return 4; } -}; - -static IKFactory::Class> bio2("bio2"); -static IKFactory::Class> bio2_memetic("bio2_memetic"); -static IKFactory::Class> bio2_memetic_l("bio2_memetic_l"); - -#ifdef ENABLE_CPP_OPTLIB -static IKFactory::Class> bio2_memetic_0("bio2_memetic_lbfgs"); -#endif -} diff --git a/src/bio_ik-kinetic-support/src/ik_gradient.cpp b/src/bio_ik-kinetic-support/src/ik_gradient.cpp deleted file mode 100644 index cd33c56..0000000 --- a/src/bio_ik-kinetic-support/src/ik_gradient.cpp +++ /dev/null @@ -1,289 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -namespace bio_ik -{ - -// pseudoinverse jacobian solver -// (mainly for testing RobotFK_Jacobian::computeJacobian) -template struct IKJacobianBase : BASE -{ - using BASE::modelInfo; - using BASE::model; - using BASE::params; - using BASE::computeFitness; - using BASE::problem; - - std::vector tipObjectives; - - Eigen::VectorXd tip_diffs; - Eigen::VectorXd joint_diffs; - Eigen::MatrixXd jacobian; - std::vector tip_frames_temp; - - IKJacobianBase(const IKParams& p) - : BASE(p) - { - } - - void initialize(const Problem& problem) - { - BASE::initialize(problem); - tipObjectives.resize(problem.tip_link_indices.size()); - for(auto& goal : problem.goals) - tipObjectives[goal.tip_index] = goal.frame; - } - - void optimizeJacobian(std::vector& solution) - { - FNPROFILER(); - - int tip_count = problem.tip_link_indices.size(); - tip_diffs.resize(tip_count * 6); - joint_diffs.resize(problem.active_variables.size()); - - // compute fk - model.applyConfiguration(solution); - - double translational_scale = 1; - double rotational_scale = 1; - - // compute goal diffs - tip_frames_temp = model.getTipFrames(); - for(int itip = 0; itip < tip_count; itip++) - { - auto twist = frameTwist(tip_frames_temp[itip], tipObjectives[itip]); - tip_diffs(itip * 6 + 0) = twist.vel.x() * translational_scale; - tip_diffs(itip * 6 + 1) = twist.vel.y() * translational_scale; - tip_diffs(itip * 6 + 2) = twist.vel.z() * translational_scale; - tip_diffs(itip * 6 + 3) = twist.rot.x() * rotational_scale; - tip_diffs(itip * 6 + 4) = twist.rot.y() * rotational_scale; - tip_diffs(itip * 6 + 5) = twist.rot.z() * rotational_scale; - } - - // compute jacobian - { - model.computeJacobian(problem.active_variables, jacobian); - int icol = 0; - for(auto ivar : problem.active_variables) - { - for(size_t itip = 0; itip < tip_count; itip++) - { - jacobian(itip * 6 + 0, icol) *= translational_scale; - jacobian(itip * 6 + 1, icol) *= translational_scale; - jacobian(itip * 6 + 2, icol) *= translational_scale; - jacobian(itip * 6 + 3, icol) *= rotational_scale; - jacobian(itip * 6 + 4, icol) *= rotational_scale; - jacobian(itip * 6 + 5, icol) *= rotational_scale; - } - icol++; - } - } - - // get pseudoinverse and multiply - joint_diffs = jacobian.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tip_diffs); - // joint_diffs = (jacobian.transpose() * jacobian).ldlt().solve(jacobian.transpose() * tip_diffs); - - // apply joint deltas and clip - { - int icol = 0; - for(auto ivar : problem.active_variables) - { - auto v = solution[ivar] + joint_diffs(icol); - if(!std::isfinite(v)) continue; - v = modelInfo.clip(v, ivar); - solution[ivar] = v; - icol++; - } - } - } -}; - -// simple gradient descent -template struct IKGradientDescent : IKBase -{ - std::vector solution, best_solution, gradient, temp; - bool reset; - - IKGradientDescent(const IKParams& p) - : IKBase(p) - { - } - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - solution = problem.initial_guess; - if(thread_index > 0) - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - best_solution = solution; - reset = false; - } - - const std::vector& getSolution() const { return best_solution; } - - void step() - { - // random reset if stuck - if(reset) - { - reset = false; - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - - // compute gradient direction - temp = solution; - double jd = 0.0001; - gradient.resize(solution.size(), 0); - for(auto ivar : problem.active_variables) - { - temp[ivar] = solution[ivar] - jd; - double p1 = computeFitness(temp); - - temp[ivar] = solution[ivar] + jd; - double p3 = computeFitness(temp); - - temp[ivar] = solution[ivar]; - - gradient[ivar] = p3 - p1; - } - - // normalize gradient direction - double sum = 0.0001; - for(auto ivar : problem.active_variables) - sum += fabs(gradient[ivar]); - double f = 1.0 / sum * jd; - for(auto ivar : problem.active_variables) - gradient[ivar] *= f; - - // initialize line search - temp = solution; - - for(auto ivar : problem.active_variables) - temp[ivar] = solution[ivar] - gradient[ivar]; - double p1 = computeFitness(temp); - - // for(auto ivar : problem.active_variables) temp[ivar] = solution[ivar]; - // double p2 = computeFitness(temp); - - for(auto ivar : problem.active_variables) - temp[ivar] = solution[ivar] + gradient[ivar]; - double p3 = computeFitness(temp); - - double p2 = (p1 + p3) * 0.5; - - // linear step size estimation - double cost_diff = (p3 - p1) * 0.5; - double joint_diff = p2 / cost_diff; - - // apply optimization step - // (move along gradient direction by estimated step size) - for(auto ivar : problem.active_variables) - temp[ivar] = modelInfo.clip(solution[ivar] - gradient[ivar] * joint_diff, ivar); - - if(if_stuck == 'c') - { - // always accept solution and continue - solution = temp; - } - else - { - // has solution improved? - if(computeFitness(temp) < computeFitness(solution)) - { - // solution improved -> accept solution - solution = temp; - } - else - { - if(if_stuck == 'r') - { - // reset if stuck - reset = true; - } - } - } - - // update best solution - if(computeFitness(solution) < computeFitness(best_solution)) best_solution = solution; - } - - size_t concurrency() const { return threads; } -}; - -static IKFactory::Class> gd("gd"); -static IKFactory::Class> gd_2("gd_2"); -static IKFactory::Class> gd_4("gd_4"); -static IKFactory::Class> gd_8("gd_8"); - -static IKFactory::Class> gd_r("gd_r"); -static IKFactory::Class> gd_2_r("gd_r_2"); -static IKFactory::Class> gd_4_r("gd_r_4"); -static IKFactory::Class> gd_8_r("gd_r_8"); - -static IKFactory::Class> gd_c("gd_c"); -static IKFactory::Class> gd_2_c("gd_c_2"); -static IKFactory::Class> gd_4_c("gd_c_4"); -static IKFactory::Class> gd_8_c("gd_c_8"); - -// pseudoinverse jacobian only -template struct IKJacobian : IKJacobianBase -{ - using IKBase::initialize; - std::vector solution; - IKJacobian(const IKParams& p) - : IKJacobianBase(p) - { - } - void initialize(const Problem& problem) - { - IKJacobianBase::initialize(problem); - solution = problem.initial_guess; - if(thread_index > 0) - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - const std::vector& getSolution() const { return solution; } - void step() { optimizeJacobian(solution); } - size_t concurrency() const { return threads; } -}; -static IKFactory::Class> jac("jac"); -static IKFactory::Class> jac_2("jac_2"); -static IKFactory::Class> jac_4("jac_4"); -static IKFactory::Class> jac_8("jac_8"); -} diff --git a/src/bio_ik-kinetic-support/src/ik_neural.cpp b/src/bio_ik-kinetic-support/src/ik_neural.cpp deleted file mode 100644 index 8c4c2a3..0000000 --- a/src/bio_ik-kinetic-support/src/ik_neural.cpp +++ /dev/null @@ -1,690 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -#include -#include - -#include - -#define LOG_LIST(l) \ - { \ - LOG(#l "[]"); \ - for(std::size_t i = 0; i < l.size(); i++) \ - { \ - LOG(#l "[" + std::to_string(i) + "]", l[i]); \ - } \ - } - -namespace bio_ik -{ - -/* -static inline KDL::Twist toTwist(const Frame& frame) -{ - KDL::Twist t; - t.vel.x(frame.pos.x()); - t.vel.y(frame.pos.y()); - t.vel.z(frame.pos.z()); - auto r = frame.rot.getAxis() * frame.rot.getAngle(); - t.rot.x(r.x()); - t.rot.y(r.y()); - t.rot.z(r.z()); - return t; -} - -static inline KDL::Twist frameTwist(const Frame& a, const Frame& b) -{ - auto frame = inverse(a) * b; - KDL::Twist t; - t.vel.x(frame.pos.x()); - t.vel.y(frame.pos.y()); - t.vel.z(frame.pos.z()); - auto r = frame.rot.getAxis() * frame.rot.getAngle(); - t.rot.x(r.x()); - t.rot.y(r.y()); - t.rot.z(r.z()); - return t; -} -*/ - -struct IKNeural : IKBase -{ - std::vector solution; - FANN::neural_net net; - - std::vector> input_minmax, output_minmax; - - static void find_minmax(const std::vector& values, std::vector>& minmax) - { - for(size_t i = 0; i < minmax.size(); i++) - { - minmax[i] = std::make_pair(values[i], values[i]); - } - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - p.first = std::min(p.first, v); - p.second = std::max(p.second, v); - } - } - - static void normalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = (v - p.first) / (p.second - p.first); - } - } - - static void denormalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = v * (p.second - p.first) + p.first; - } - } - - unsigned int input_count, output_count; - - IKNeural(const IKParams& p) - : IKBase(p) - { - trained = false; - } - - bool trained; - - void train() - { - trained = true; - - input_count = problem.active_variables.size() + problem.tip_link_indices.size() * 6; - output_count = problem.active_variables.size(); - - LOG_VAR(input_count); - LOG_VAR(output_count); - - // std::vector levels = {input_count, input_count, input_count, output_count}; - - // std::vector levels = {input_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, input_count + output_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, 100, output_count}; - - std::vector levels = {input_count, 50, output_count}; - - net.create_standard_array(levels.size(), levels.data()); - - size_t var_count = params.robot_model->getVariableCount(); - std::vector state1(var_count), state2(var_count); - std::vector frames1, frames2; - - std::vector inputs, outputs; - std::vector input_pp, output_pp; - - LOG("neuro ik generating training data"); - - unsigned int samples = 10000; - - for(size_t iter = 0; iter < samples; iter++) - { - for(size_t ivar = 0; ivar < var_count; ivar++) - { - state1[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar)); - state1[ivar] = modelInfo.clip(state1[ivar], ivar); - // state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * modelInfo.getSpan(ivar), ivar); - state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * 0.1, ivar); - } - - model.applyConfiguration(state1); - frames1 = model.getTipFrames(); - model.applyConfiguration(state2); - frames2 = model.getTipFrames(); - - for(auto ivar : problem.active_variables) - { - inputs.push_back(state1[ivar]); - outputs.push_back(state2[ivar] - state1[ivar]); - } - - for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++) - { - double translational_scale = 1.0; - double rotational_scale = 1.0; - - // Frame diff = inverse(frames1[itip]) * frames2[itip]; - // auto twist = toTwist(diff); - auto twist = frameTwist(frames1[itip], frames2[itip]); - - inputs.push_back(frames2[itip].pos.x() - frames1[itip].pos.x()); - inputs.push_back(frames2[itip].pos.y() - frames1[itip].pos.y()); - inputs.push_back(frames2[itip].pos.z() - frames1[itip].pos.z()); - - inputs.push_back(twist.rot.x() * rotational_scale); - inputs.push_back(twist.rot.y() * rotational_scale); - inputs.push_back(twist.rot.z() * rotational_scale); - } - } - - for(auto& v : inputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - for(auto& v : outputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - - input_minmax.resize(input_count); - output_minmax.resize(output_count); - - find_minmax(inputs, input_minmax); - find_minmax(outputs, output_minmax); - - normalize(inputs, input_minmax); - normalize(outputs, output_minmax); - - for(size_t iter = 0; iter < samples; iter++) - { - input_pp.push_back(inputs.data() + iter * input_count); - output_pp.push_back(outputs.data() + iter * output_count); - } - - LOG("neuro ik training"); - - FANN::training_data train; - train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data()); - net.set_callback( - [](FANN::neural_net& net, FANN::training_data& train, unsigned int max_epochs, unsigned int epochs_between_reports, float desired_error, unsigned int epochs, void* user_data) { - if(epochs % epochs_between_reports != 0) return 0; - // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%"); - LOG("training", epochs, net.get_MSE(), desired_error); - return 0; - }, - 0); - - net.set_activation_function_hidden(FANN::SIGMOID); - net.set_activation_function_output(FANN::SIGMOID); - - net.init_weights(train); - - net.train_on_data(train, 1000, 1, 0.0001); - - fann_type err = net.test_data(train); - LOG("neuro ik training error:", err); - - /*std::vector iiv, oov, ttv; - for(size_t iter = 0; iter < 10; iter++) - { - auto* ii = input_pp[iter]; - auto* oo = net.run(ii); - auto* tt = output_pp[iter]; - iiv.assign(ii, ii + input_count); - ttv.assign(tt, tt + output_count); - oov.assign(oo, oo + output_count); - LOG_LIST(iiv); - LOG_LIST(ttv); - LOG_LIST(oov); - }*/ - - LOG("training done"); - } - - size_t iterations = 0; - - void initialize(const Problem& problem) - { - static std::mutex mutex; - std::lock_guard lock(mutex); - IKBase::initialize(problem); - solution = problem.initial_guess; - if(!trained) train(); - iterations = 0; - if(thread_index > 0) - for(auto& vi : problem.active_variables) - solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi)); - } - - const std::vector& getSolution() const { return solution; } - - std::vector inputs, outputs; - - std::vector tip_objectives; - - /*void step() - { - //if(iterations > 1) return; - iterations++; - - inputs.clear(); - for(auto ivar : problem.active_variables) - { - inputs.push_back(solution[ivar]); - } - - tip_objectives.resize(model.getTipFrames().size()); - for(auto& g : problem.goals) - { - tip_objectives[g.tip_index] = g.frame; - } - - model.applyConfiguration(solution); - auto& frames1 = model.getTipFrames(); - auto& frames2 = tip_objectives; - - double scale = 1.0; - - for(size_t itip = 0; itip < tip_objectives.size(); itip++) - { - double translational_scale = 1.0; - double rotational_scale = 1.0; - - //Frame diff = inverse(frames1[itip]) * frames2[itip]; - //auto twist = toTwist(diff); - auto twist = frameTwist(frames1[itip], frames2[itip]); - - auto dpos = frames2[itip].pos - frames1[itip].pos; - auto drot = Vector3( - twist.rot.x() * rotational_scale, - twist.rot.y() * rotational_scale, - twist.rot.z() * rotational_scale - ); - - scale = 1.0 / (0.0000001 + dpos.length() + drot.length()); - - dpos = dpos * scale; - drot = drot * scale; - - inputs.push_back(dpos.x()); - inputs.push_back(dpos.y()); - inputs.push_back(dpos.z()); - - inputs.push_back(drot.x()); - inputs.push_back(drot.y()); - inputs.push_back(drot.z()); - } - - normalize(inputs, input_minmax); - - auto* oo = net.run(inputs.data()); - - outputs.assign(oo, oo + output_count); - - denormalize(outputs, output_minmax); - - auto& vv = problem.active_variables; - for(size_t iout = 0; iout < vv.size(); iout++) - { - size_t ivar = vv[iout]; - solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 0.1 / scale, ivar); - } - }*/ - - void step() - { - // if(iterations > 1) return; - iterations++; - - inputs.clear(); - for(auto ivar : problem.active_variables) - { - inputs.push_back(solution[ivar]); - } - - tip_objectives.resize(model.getTipFrames().size()); - for(auto& g : problem.goals) - { - tip_objectives[g.tip_index] = g.frame; - } - - model.applyConfiguration(solution); - auto& frames1 = model.getTipFrames(); - auto& frames2 = tip_objectives; - - double scale = 1.0; - for(size_t itip = 0; itip < tip_objectives.size(); itip++) - { - double translational_scale = 1.0; - double rotational_scale = 1.0; - auto twist = frameTwist(frames1[itip], frames2[itip]); - auto dpos = frames2[itip].pos - frames1[itip].pos; - auto drot = Vector3(twist.rot.x() * rotational_scale, twist.rot.y() * rotational_scale, twist.rot.z() * rotational_scale); - - /*if(iterations % 2) - { - scale = 1.0 / (0.0000001 + dpos.length()); - inputs.push_back(dpos.x() * scale); - inputs.push_back(dpos.y() * scale); - inputs.push_back(dpos.z() * scale); - inputs.push_back(0); - inputs.push_back(0); - inputs.push_back(0); - } else { - scale = 1.0 / (0.0000001 + drot.length()); - inputs.push_back(0); - inputs.push_back(0); - inputs.push_back(0); - inputs.push_back(drot.x() * scale); - inputs.push_back(drot.y() * scale); - inputs.push_back(drot.z() * scale); - }*/ - - { - scale = 1.0 / (0.0000001 + dpos.length() + drot.length()); - inputs.push_back(dpos.x() * scale); - inputs.push_back(dpos.y() * scale); - inputs.push_back(dpos.z() * scale); - inputs.push_back(drot.x() * scale); - inputs.push_back(drot.y() * scale); - inputs.push_back(drot.z() * scale); - } - } - normalize(inputs, input_minmax); - auto* oo = net.run(inputs.data()); - outputs.assign(oo, oo + output_count); - denormalize(outputs, output_minmax); - auto& vv = problem.active_variables; - for(size_t iout = 0; iout < vv.size(); iout++) - { - size_t ivar = vv[iout]; - solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 1 / scale, ivar); - } - } -}; - -static IKFactory::Class neural("neural"); - -struct IKNeural2 : IKBase -{ - std::vector solution; - FANN::neural_net net; - - std::vector> input_minmax, output_minmax; - - /*static void find_minmax(const std::vector& values, std::vector>& minmax) - { - for(size_t i = 0; i < minmax.size(); i++) - { - minmax[i] = std::make_pair(values[i], values[i]); - } - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - p.first = std::min(p.first, v); - p.second = std::max(p.second, v); - } - }*/ - - static void find_minmax(const std::vector& values, std::vector>& minmax) - { - std::vector centers(minmax.size(), 0.0); - for(size_t i = 0; i < values.size(); i++) - centers[i % minmax.size()] += values[i] * (1.0 * minmax.size() / values.size()); - - std::vector ranges2(minmax.size(), 0.0001); - for(size_t i = 0; i < values.size(); i++) - { - double d = values[i] - centers[i % minmax.size()]; - d = d * d; - ranges2[i % minmax.size()] += d * (1.0 * minmax.size() / values.size()); - } - - for(size_t i = 0; i < minmax.size(); i++) - { - auto& p = minmax[i]; - p.first = centers[i] - sqrt(ranges2[i]); - p.second = centers[i] + sqrt(ranges2[i]); - } - } - - static void normalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = (v - p.first) / (p.second - p.first); - } - } - - static void denormalize(std::vector& values, const std::vector>& minmax) - { - for(size_t i = 0; i < values.size(); i++) - { - auto& v = values[i]; - auto& p = minmax[i % minmax.size()]; - v = v * (p.second - p.first) + p.first; - } - } - - unsigned int input_count, output_count; - - IKNeural2(const IKParams& p) - : IKBase(p) - { - trained = false; - } - - bool trained; - - void train() - { - trained = true; - - input_count = problem.tip_link_indices.size() * 7; - output_count = problem.active_variables.size(); - - LOG_VAR(input_count); - LOG_VAR(output_count); - - // std::vector levels = {input_count, 100, 100, output_count}; - - // std::vector levels = {input_count, input_count, input_count, output_count}; - - std::vector levels = {input_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, input_count + output_count, input_count + output_count, output_count}; - - // std::vector levels = {input_count, output_count}; - - net.create_standard_array(levels.size(), levels.data()); - - size_t var_count = params.robot_model->getVariableCount(); - std::vector state = problem.initial_guess; - std::vector frames; - - std::vector inputs, outputs; - std::vector input_pp, output_pp; - - LOG("neuro ik generating training data"); - - unsigned int samples = 10000; - - for(size_t iter = 0; iter < samples; iter++) - { - for(size_t ivar : problem.active_variables) - state[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar)); - - model.applyConfiguration(state); - frames = model.getTipFrames(); - - for(auto ivar : problem.active_variables) - outputs.push_back(state[ivar]); - - for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++) - { - inputs.push_back(frames[itip].pos.x()); - inputs.push_back(frames[itip].pos.y()); - inputs.push_back(frames[itip].pos.z()); - - auto rot = frames[itip].rot; - rot = rot * rot; - // rot = tf::Quaternion(0, 0, 0, 1); - inputs.push_back(rot.x()); - inputs.push_back(rot.y()); - inputs.push_back(rot.z()); - inputs.push_back(rot.w()); - } - } - - for(auto& v : inputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - for(auto& v : outputs) - if(!std::isfinite(v)) throw std::runtime_error("NAN"); - - input_minmax.resize(input_count); - output_minmax.resize(output_count); - - find_minmax(inputs, input_minmax); - find_minmax(outputs, output_minmax); - - normalize(inputs, input_minmax); - normalize(outputs, output_minmax); - - for(size_t iter = 0; iter < samples; iter++) - { - input_pp.push_back(inputs.data() + iter * input_count); - output_pp.push_back(outputs.data() + iter * output_count); - } - - LOG("neuro ik training"); - - FANN::training_data train; - train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data()); - net.set_callback( - [](FANN::neural_net& net, FANN::training_data& train, unsigned int max_epochs, unsigned int epochs_between_reports, float desired_error, unsigned int epochs, void* user_data) { - if(epochs % epochs_between_reports != 0) return 0; - // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%"); - LOG("training", epochs, net.get_MSE(), desired_error); - return 0; - }, - 0); - - net.set_activation_function_hidden(FANN::SIGMOID); - net.set_activation_function_output(FANN::SIGMOID); - - net.init_weights(train); - - net.train_on_data(train, 100, 1, 0.0001); - - fann_type err = net.test_data(train); - LOG("neuro ik training error:", err); - - /*std::vector iiv, oov, ttv; - for(size_t iter = 0; iter < 10; iter++) - { - auto* ii = input_pp[iter]; - auto* oo = net.run(ii); - auto* tt = output_pp[iter]; - iiv.assign(ii, ii + input_count); - ttv.assign(tt, tt + output_count); - oov.assign(oo, oo + output_count); - LOG_LIST(iiv); - LOG_LIST(ttv); - LOG_LIST(oov); - }*/ - - LOG("training done"); - } - - size_t iterations = 0; - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - solution = problem.initial_guess; - if(!trained) train(); - iterations = 0; - } - - const std::vector& getSolution() const { return solution; } - - std::vector inputs, outputs; - - std::vector tip_objectives; - - void step() - { - if(iterations > 1) return; - iterations++; - - inputs.clear(); - - tip_objectives.resize(model.getTipFrames().size()); - for(auto& g : problem.goals) - { - tip_objectives[g.tip_index] = g.frame; - } - - auto& frames = tip_objectives; - - for(size_t itip = 0; itip < tip_objectives.size(); itip++) - { - inputs.push_back(frames[itip].pos.x()); - inputs.push_back(frames[itip].pos.y()); - inputs.push_back(frames[itip].pos.z()); - - auto rot = frames[itip].rot; - rot = rot * rot; - // rot = tf::Quaternion(0, 0, 0, 1); - inputs.push_back(rot.x()); - inputs.push_back(rot.y()); - inputs.push_back(rot.z()); - inputs.push_back(rot.w()); - } - - normalize(inputs, input_minmax); - - auto* oo = net.run(inputs.data()); - - outputs.assign(oo, oo + output_count); - - denormalize(outputs, output_minmax); - - auto& vv = problem.active_variables; - for(size_t iout = 0; iout < vv.size(); iout++) - { - size_t ivar = vv[iout]; - solution[ivar] = modelInfo.clip(outputs[iout], ivar); - } - } -}; - -static IKFactory::Class neural2("neural2"); -} diff --git a/src/bio_ik-kinetic-support/src/ik_parallel.h b/src/bio_ik-kinetic-support/src/ik_parallel.h deleted file mode 100644 index 8197ca6..0000000 --- a/src/bio_ik-kinetic-support/src/ik_parallel.h +++ /dev/null @@ -1,278 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -#include - -namespace bio_ik -{ - -// executes a function in parallel on pre-allocated threads -class ParallelExecutor -{ - std::function fun; - std::vector threads; - boost::barrier barrier; - volatile bool exit; - double best_fitness; - -public: - template - ParallelExecutor(size_t thread_count, const FUN& f) - : exit(false) - , threads(thread_count) - , fun(f) - , barrier(thread_count) - { - for(size_t i = 1; i < thread_count; i++) - { - std::thread t([this, i]() { - while(true) - { - barrier.wait(); - if(exit) break; - fun(i); - barrier.wait(); - if(exit) break; - } - }); - std::swap(t, threads[i]); - } - } - ~ParallelExecutor() - { - exit = true; - barrier.wait(); - for(auto& t : threads) - if(t.joinable()) t.join(); - } - void run() - { - barrier.wait(); - fun(0); - barrier.wait(); - } -}; - -// runs ik on multiple threads until a stop criterion is met -struct IKParallel -{ - IKParams params; - std::vector> solvers; - std::vector> solver_solutions; - std::vector> solver_temps; - std::vector solver_success; - std::vector solver_fitness; - int thread_count; - // std::vector fk; // TODO: remove - double timeout; - bool success; - std::atomic finished; - std::atomic iteration_count; - std::vector result; - std::unique_ptr par; - Problem problem; - bool enable_counter; - double best_fitness; - - IKParallel(const IKParams& params) - : params(params) - { - // solver class name - std::string name = params.solver_class_name; - - enable_counter = params.enable_counter; - - // create solvers - solvers.emplace_back(IKFactory::create(name, params)); - thread_count = solvers.front()->concurrency(); - if(params.thread_count) { - thread_count = params.thread_count; - } - while(solvers.size() < thread_count) - solvers.emplace_back(IKFactory::clone(solvers.front().get())); - for(size_t i = 0; i < thread_count; i++) - solvers[i]->thread_index = i; - - // while(fk.size() < thread_count) fk.emplace_back(params.robot_model); - - // init buffers - solver_solutions.resize(thread_count); - solver_temps.resize(thread_count); - solver_success.resize(thread_count); - solver_fitness.resize(thread_count); - - // create parallel executor - par.reset(new ParallelExecutor(thread_count, [this](size_t i) { solverthread(i); })); - } - - void initialize(const Problem& problem) - { - this->problem = problem; - // for(auto& f : fk) f.initialize(problem.tip_link_indices); - } - -private: - void solverthread(size_t i) - { - THREADPROFILER("thread", i); - COUNTERPROFILER("solver threads"); - - // initialize ik solvers - { - BLOCKPROFILER("ik solver init"); - solvers[i]->initialize(problem); - } - - // run solver iterations until solution found or timeout - for(size_t iteration = 0; (ros::WallTime::now().toSec() < timeout && finished == 0) || (iteration == 0 && i == 0); iteration++) - { - if(finished) break; - - // run solver for a few steps - solvers[i]->step(); - iteration_count++; - for(int it2 = 1; it2 < 4; it2++) - if(ros::WallTime::now().toSec() < timeout && finished == 0) solvers[i]->step(); - - if(finished) break; - - // get solution and check stop criterion - auto& result = solver_temps[i]; - result = solvers[i]->getSolution(); - auto& fk = solvers[i]->model; - fk.applyConfiguration(result); - bool success = solvers[i]->checkSolution(result, fk.getTipFrames()); - if(success) finished = 1; - solver_success[i] = success; - solver_solutions[i] = result; - solver_fitness[i] = solvers[i]->computeFitness(result, fk.getTipFrames()); - - if(success) break; - } - - finished = 1; - - for(auto& s : solvers) - s->canceled = true; - } - -public: - void solve() - { - BLOCKPROFILER("solve mt"); - - // prepare - iteration_count = 0; - result = problem.initial_guess; - timeout = problem.timeout; - success = false; - finished = 0; - for(auto& s : solver_solutions) - s = problem.initial_guess; - for(auto& s : solver_temps) - s = problem.initial_guess; - for(auto& s : solver_success) - s = 0; - for(auto& f : solver_fitness) - f = DBL_MAX; - for(auto& s : solvers) - s->canceled = false; - - // run solvers - { - BLOCKPROFILER("solve mt 2"); - par->run(); - } - - size_t best_index = 0; - best_fitness = DBL_MAX; - - // if exact primary goal matches have been found ... - for(size_t i = 0; i < thread_count; i++) - { - if(solver_success[i]) - { - double fitness; - if(solvers[0]->problem.secondary_goals.empty()) - { - // ... and if no secondary goals have been specified, - // select the best result according to primary goals - fitness = solver_fitness[i]; - } - else - { - // ... and if secondary goals have been specified, - // select the result that best satisfies primary and secondary goals - fitness = solver_fitness[i] + solvers[0]->computeSecondaryFitnessAllVariables(solver_solutions[i]); - } - if(fitness < best_fitness) - { - best_fitness = fitness; - best_index = i; - } - } - } - - // if no exact primary goal matches have been found, - // select best primary goal approximation - if(best_fitness == DBL_MAX) - { - for(size_t i = 0; i < thread_count; i++) - { - if(solver_fitness[i] < best_fitness) - { - best_fitness = solver_fitness[i]; - best_index = i; - } - } - } - - if(enable_counter) - { - LOG("iterations", iteration_count); - } - - result = solver_solutions[best_index]; - success = solver_success[best_index]; - } - - double getSolutionFitness() const { return best_fitness; } - - bool getSuccess() const { return success; } - - const std::vector& getSolution() const { return result; } -}; -} diff --git a/src/bio_ik-kinetic-support/src/ik_test.cpp b/src/bio_ik-kinetic-support/src/ik_test.cpp deleted file mode 100644 index f1a07ec..0000000 --- a/src/bio_ik-kinetic-support/src/ik_test.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -namespace bio_ik -{ - -struct IKTest : IKBase -{ - - RobotFK_MoveIt fkref; - - std::vector temp; - - double d_rot_sum, d_pos_sum, d_div; - - IKTest(const IKParams& params) - : IKBase(params) - , fkref(params.robot_model) - { - d_rot_sum = d_pos_sum = d_div = 0; - } - - /*double tipdiff(const std::vector& fa, const std::vector& fb) - { - double diff = 0.0; - for(size_t i = 0; i < problem.tip_link_indices.size(); i++) - { - //LOG_VAR(fa[i]); - //LOG_VAR(fb[i]); - diff += fa[i].rot.angleShortestPath(fb[i].rot); - diff += fa[i].pos.distance(fb[i].pos); - } - return diff; - }*/ - - void initialize(const Problem& problem) - { - IKBase::initialize(problem); - - fkref.initialize(problem.tip_link_indices); - model.initialize(problem.tip_link_indices); - - fkref.applyConfiguration(problem.initial_guess); - model.applyConfiguration(problem.initial_guess); - - // double diff = tipdiff(fkref.getTipFrames(), model.getTipFrames()); - // LOG_VAR(diff); - - /*{ - auto& fa = fkref.getTipFrames(); - auto& fb = model.getTipFrames(); - for(size_t i = 0; i < problem.tip_link_indices.size(); i++) - { - LOG("d rot", i, fa[i].rot.angleShortestPath(fb[i].rot)); - LOG("d pos", i, fa[i].pos.distance(fb[i].pos)); - } - }*/ - - { - temp = problem.initial_guess; - for(size_t ivar : problem.active_variables) - if(modelInfo.isRevolute(ivar) || modelInfo.isPrismatic(ivar)) temp[ivar] = modelInfo.clip(temp[ivar] + random(-0.1, 0.1), ivar); - - fkref.applyConfiguration(temp); - auto& fa = fkref.getTipFrames(); - - model.applyConfiguration(problem.initial_guess); - model.initializeMutationApproximator(problem.active_variables); - - std::vector> fbm; - - std::vector mutation_values; - for(size_t ivar : problem.active_variables) - mutation_values.push_back(temp[ivar]); - const double* mutation_ptr = mutation_values.data(); - - model.computeApproximateMutations(1, &mutation_ptr, fbm); - - auto& fb = fbm[0]; - - // auto& fb = model.getTipFrames(); - - for(size_t i = 0; i < problem.tip_link_indices.size(); i++) - { - // LOG("d rot", i, fa[i].rot.angleShortestPath(fb[i].rot)); - // LOG("d pos", i, fa[i].pos.distance(fb[i].pos)); - - d_rot_sum += fa[i].rot.angleShortestPath(fb[i].rot); - d_pos_sum += fa[i].pos.distance(fb[i].pos); - d_div += 1; - } - } - - LOG("d rot avg", d_rot_sum / d_div); - LOG("d pos avg", d_pos_sum / d_div); - } - - void step() {} - - const std::vector& getSolution() const { return problem.initial_guess; } -}; - -static IKFactory::Class test("test"); -} diff --git a/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp b/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp deleted file mode 100644 index 0f5049b..0000000 --- a/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp +++ /dev/null @@ -1,649 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include - -#include "forward_kinematics.h" -#include "ik_base.h" -#include "ik_parallel.h" -#include "problem.h" -#include "utils.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -//#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -using namespace bio_ik; - -// implement BioIKKinematicsQueryOptions - -namespace bio_ik { - -std::mutex bioIKKinematicsQueryOptionsMutex; -std::unordered_set bioIKKinematicsQueryOptionsList; - -BioIKKinematicsQueryOptions::BioIKKinematicsQueryOptions() - : replace(false), solution_fitness(0) { - std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); - bioIKKinematicsQueryOptionsList.insert(this); -} - -BioIKKinematicsQueryOptions::~BioIKKinematicsQueryOptions() { - std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); - bioIKKinematicsQueryOptionsList.erase(this); -} - -bool isBioIKKinematicsQueryOptions(const void *ptr) { - std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); - return bioIKKinematicsQueryOptionsList.find(ptr) != - bioIKKinematicsQueryOptionsList.end(); -} - -const BioIKKinematicsQueryOptions * -toBioIKKinematicsQueryOptions(const void *ptr) { - if (isBioIKKinematicsQueryOptions(ptr)) - return (const BioIKKinematicsQueryOptions *)ptr; - else - return 0; -} - -} // namespace bio_ik - -// BioIK Kinematics Plugin - -namespace bio_ik_kinematics_plugin { - -// Fallback for older MoveIt versions which don't support lookupParam yet -template -static void lookupParam(const std::string ¶m, T &val, - const T &default_val) { - ros::NodeHandle nodeHandle("~"); - val = nodeHandle.param(param, default_val); -} - -struct BioIKKinematicsPlugin : kinematics::KinematicsBase { - std::vector joint_names, link_names; - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup *joint_model_group; - mutable std::unique_ptr ik; - mutable std::vector state, temp; - mutable std::unique_ptr temp_state; - mutable std::vector tipFrames; - RobotInfo robot_info; - bool enable_profiler; - - BioIKKinematicsPlugin() { enable_profiler = false; } - - virtual const std::vector &getJointNames() const { - LOG_FNC(); - return joint_names; - } - - virtual const std::vector &getLinkNames() const { - LOG_FNC(); - return link_names; - } - - virtual bool getPositionFK(const std::vector &link_names, - const std::vector &joint_angles, - std::vector &poses) const { - LOG_FNC(); - return false; - } - - virtual 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 { - LOG_FNC(); - return false; - } - - std::vector tip_reference_frames; - - mutable std::vector> default_goals; - - mutable std::vector all_goals; - - IKParams ikparams; - - mutable Problem problem; - - static moveit::core::RobotModelConstPtr - loadRobotModel(const std::string &robot_description) { - static std::map - robot_model_cache; - static std::mutex cache_mutex; - std::lock_guard lock(cache_mutex); - if (robot_model_cache.find(robot_description) == robot_model_cache.end()) { - rdf_loader::RDFLoader rdf_loader(robot_description); - auto srdf = rdf_loader.getSRDF(); - auto urdf_model = rdf_loader.getURDF(); - - if (!urdf_model || !srdf) { - LOG("URDF and SRDF must be loaded for kinematics solver to work."); - return nullptr; - } - robot_model_cache[robot_description] = moveit::core::RobotModelConstPtr( - new robot_model::RobotModel(urdf_model, srdf)); - } - return robot_model_cache[robot_description]; - - // return - // moveit::planning_interface::getSharedRobotModel(robot_description); - } - - bool load(std::string robot_description, std::string group_name) { - LOG_FNC(); - - // LOG_VAR(robot_description); - // LOG_VAR(group_name); - - LOG("bio ik init", ros::this_node::getName()); - - /*rdf_loader::RDFLoader rdf_loader(robot_description_); - auto srdf = rdf_loader.getSRDF(); - auto urdf_model = rdf_loader.getURDF(); - - if(!urdf_model || !srdf) - { - LOG("URDF and SRDF must be loaded for kinematics solver to work."); - return false; - } - - robot_model.reset(new robot_model::RobotModel(urdf_model, srdf));*/ - - robot_model = loadRobotModel(robot_description); - - joint_model_group = robot_model->getJointModelGroup(group_name); - if (!joint_model_group) { - LOG("failed to get joint model group"); - return false; - } - - joint_names.clear(); - - for (auto *joint_model : joint_model_group->getJointModels()) - if (joint_model->getName() != base_frame_ && - joint_model->getType() != moveit::core::JointModel::UNKNOWN && - joint_model->getType() != moveit::core::JointModel::FIXED) - joint_names.push_back(joint_model->getName()); - - auto tips2 = tip_frames_; - joint_model_group->getEndEffectorTips(tips2); - if (!tips2.empty()) - tip_frames_ = tips2; - - link_names = tip_frames_; - - // for(auto& n : joint_names) LOG("joint", n); - // for(auto& n : link_names) LOG("link", n); - - // bool enable_profiler; - lookupParam("profiler", enable_profiler, false); - // if(enable_profiler) Profiler::start(); - - robot_info = RobotInfo(robot_model); - - ikparams.robot_model = robot_model; - ikparams.joint_model_group = joint_model_group; - - // initialize parameters for IKParallel - lookupParam("mode", ikparams.solver_class_name, - std::string("bio2_memetic")); - lookupParam("counter", ikparams.enable_counter, false); - lookupParam("threads", ikparams.thread_count, 0); - - // initialize parameters for Problem - lookupParam("dpos", ikparams.dpos, DBL_MAX); - lookupParam("drot", ikparams.drot, DBL_MAX); - lookupParam("dtwist", ikparams.dtwist, 1e-5); - - // initialize parameters for ik_evolution_1 - lookupParam("no_wipeout", ikparams.opt_no_wipeout, false); - lookupParam("population_size", ikparams.population_size, 8); - lookupParam("elite_count", ikparams.elite_count, 4); - lookupParam("linear_fitness", ikparams.linear_fitness, false); - - temp_state.reset(new moveit::core::RobotState(robot_model)); - - ik.reset(new IKParallel(ikparams)); - - { - - BLOCKPROFILER("default ik goals"); - - default_goals.clear(); - - for (size_t i = 0; i < tip_frames_.size(); i++) { - PoseGoal *goal = new PoseGoal(); - - goal->setLinkName(tip_frames_[i]); - - // LOG_VAR(goal->link_name); - - double rotation_scale = 0.5; - - lookupParam("rotation_scale", rotation_scale, rotation_scale); - - bool position_only_ik = false; - lookupParam("position_only_ik", position_only_ik, position_only_ik); - if (position_only_ik) - rotation_scale = 0; - - goal->setRotationScale(rotation_scale); - - default_goals.emplace_back(goal); - } - - { - double weight = 0; - lookupParam("center_joints_weight", weight, weight); - if (weight > 0.0) { - auto *avoid_joint_limits_goal = new bio_ik::CenterJointsGoal(); - avoid_joint_limits_goal->setWeight(weight); - default_goals.emplace_back(avoid_joint_limits_goal); - } - } - - { - double weight = 0; - lookupParam("avoid_joint_limits_weight", weight, weight); - if (weight > 0.0) { - auto *avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal(); - avoid_joint_limits_goal->setWeight(weight); - default_goals.emplace_back(avoid_joint_limits_goal); - } - } - - { - double weight = 0; - lookupParam("minimal_displacement_weight", weight, weight); - if (weight > 0.0) { - auto *minimal_displacement_goal = - new bio_ik::MinimalDisplacementGoal(); - minimal_displacement_goal->setWeight(weight); - default_goals.emplace_back(minimal_displacement_goal); - } - } - } - - // LOG("init ready"); - - return true; - } - - virtual bool initialize(const std::string &robot_description, - const std::string &group_name, - const std::string &base_frame, - const std::string &tip_frame, - double search_discretization) { - LOG_FNC(); - std::vector tip_frames; - tip_frames.push_back(tip_frame); - initialize(robot_description, group_name, base_frame, tip_frames, - search_discretization); - return true; - } - - virtual bool initialize(const std::string &robot_description, - const std::string &group_name, - const std::string &base_frame, - const std::vector &tip_frames, - double search_discretization) { - LOG_FNC(); - setValues(robot_description, group_name, base_frame, tip_frames, - search_discretization); - load(robot_description, group_name); - return true; - } - - virtual 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 { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, std::vector(), - solution, IKCallbackFn(), error_code, options); - } - - virtual 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 { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, consistency_limits, - solution, IKCallbackFn(), error_code, options); - } - - virtual 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 { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, std::vector(), - solution, solution_callback, error_code, options); - } - - virtual 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 { - LOG_FNC(); - return searchPositionIK(std::vector{ik_pose}, - ik_seed_state, timeout, consistency_limits, - solution, solution_callback, error_code, options); - } - - /*struct OptMod : kinematics::KinematicsQueryOptions - { - int test; - };*/ - - virtual bool - searchPositionIK(const std::vector &ik_poses, - 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 moveit::core::RobotState *context_state = NULL) const { - double t0 = ros::WallTime::now().toSec(); - - // timeout = 0.1; - - // LOG("a"); - - if (enable_profiler) - Profiler::start(); - - auto *bio_ik_options = toBioIKKinematicsQueryOptions(&options); - - LOG_FNC(); - - FNPROFILER(); - - // LOG(typeid(options).name()); - // LOG(((OptMod*)&options)->test); - - // get variable default positions / context state - state.resize(robot_model->getVariableCount()); - robot_model->getVariableDefaultPositions(state); - if (context_state) - for (size_t i = 0; i < robot_model->getVariableCount(); i++) - state[i] = context_state->getVariablePositions()[i]; - - // overwrite used variables with seed state - solution = ik_seed_state; - { - int i = 0; - for (auto &joint_name : getJointNames()) { - auto *joint_model = robot_model->getJointModel(joint_name); - if (!joint_model) - continue; - for (size_t vi = 0; vi < joint_model->getVariableCount(); vi++) - state.at(joint_model->getFirstVariableIndex() + vi) = - solution.at(i++); - } - } - - if (!bio_ik_options || !bio_ik_options->replace) { - // transform tips to baseframe - tipFrames.clear(); - for (size_t i = 0; i < ik_poses.size(); i++) { - Eigen::Affine3d p, r; - tf::poseMsgToEigen(ik_poses[i], p); - if (context_state) { - r = context_state->getGlobalLinkTransform(getBaseFrame()); - } else { - if (i == 0) - temp_state->setToDefaultValues(); - r = temp_state->getGlobalLinkTransform(getBaseFrame()); - } - tipFrames.emplace_back(r * p); - } - } - - // init ik - - problem.timeout = t0 + timeout; - problem.initial_guess = state; - - // for(auto& v : state) LOG("var", &v - &state.front(), v); - - // problem.tip_objectives = tipFrames; - - /*for(size_t i = 0; i < problem.goals.size(); i++) - { - problem.goals[i].frame = tipFrames[i]; - }*/ - - // LOG("---"); - - /*{ - BLOCKPROFILER("ik goals"); - std::vector> goals; - for(size_t i = 0; i < tip_frames_.size(); i++) - { - //if(rand() % 2) break; - PoseGoal* goal = new PoseGoal(); - goal->link_name = tip_frames_[i]; - goal->position = tipFrames[i].pos; - goal->orientation = tipFrames[i].rot; - goals.emplace_back(goal); - //if(rand() % 20) break; - } - //std::random_shuffle(goals.begin(), goals.end()); - //LOG_VAR(goals.size()); - setRequestGoals(problem, goals, ikparams); - }*/ - - { - - if (!bio_ik_options || !bio_ik_options->replace) { - for (size_t i = 0; i < tip_frames_.size(); i++) { - auto *goal = (PoseGoal *)default_goals[i].get(); - goal->setPosition(tipFrames[i].pos); - goal->setOrientation(tipFrames[i].rot); - } - } - - all_goals.clear(); - - if (!bio_ik_options || !bio_ik_options->replace) - for (auto &goal : default_goals) - all_goals.push_back(goal.get()); - - if (bio_ik_options) - for (auto &goal : bio_ik_options->goals) - all_goals.push_back(goal.get()); - - { - BLOCKPROFILER("problem init"); - problem.initialize(ikparams.robot_model, ikparams.joint_model_group, - ikparams, all_goals, bio_ik_options); - // problem.setGoals(default_goals, ikparams); - } - } - - { - BLOCKPROFILER("ik init"); - ik->initialize(problem); - } - - // run ik solver - { - BLOCKPROFILER("ik_solve"); - ik->solve(); - } - - // get solution - state = ik->getSolution(); - - // wrap angles - for (auto ivar : problem.active_variables) { - auto v = state[ivar]; - if (robot_info.isRevolute(ivar) && - robot_model->getMimicJointModels().empty()) { - auto r = problem.initial_guess[ivar]; - auto lo = robot_info.getMin(ivar); - auto hi = robot_info.getMax(ivar); - - // move close to initial guess - if (r < v - M_PI || r > v + M_PI) { - v -= r; - v /= (2 * M_PI); - v += 0.5; - v -= std::floor(v); - v -= 0.5; - v *= (2 * M_PI); - v += r; - } - - // wrap at joint limits - if (v > hi) - v -= std::ceil(std::max(0.0, v - hi) / (2 * M_PI)) * (2 * M_PI); - if (v < lo) - v += std::ceil(std::max(0.0, lo - v) / (2 * M_PI)) * (2 * M_PI); - - // clamp at edges - if (v < lo) - v = lo; - if (v > hi) - v = hi; - } - state[ivar] = v; - } - - // wrap angles - robot_model->enforcePositionBounds(state.data()); - - // map result to jointgroup variables - { - solution.clear(); - for (auto &joint_name : getJointNames()) { - auto *joint_model = robot_model->getJointModel(joint_name); - if (!joint_model) - continue; - for (size_t vi = 0; vi < joint_model->getVariableCount(); vi++) - solution.push_back( - state.at(joint_model->getFirstVariableIndex() + vi)); - } - } - - // set solution fitness - if (bio_ik_options) { - bio_ik_options->solution_fitness = ik->getSolutionFitness(); - } - - // return an error if an accurate solution was requested, but no accurate - // solution was found - if (!ik->getSuccess() && !options.return_approximate_solution) { - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - // callback? - if (!solution_callback.empty()) { - // run callback - solution_callback(ik_poses.front(), solution, error_code); - - // return success if callback has accepted the solution - return error_code.val == error_code.SUCCESS; - } else { - // return success - error_code.val = error_code.SUCCESS; - return true; - } - } - - virtual bool supportsGroup(const moveit::core::JointModelGroup *jmg, - std::string *error_text_out = 0) const { - LOG_FNC(); - // LOG_VAR(jmg->getName()); - return true; - } -}; -} // namespace bio_ik_kinematics_plugin - -// register plugin - -#undef LOG -#undef ERROR -PLUGINLIB_EXPORT_CLASS(bio_ik_kinematics_plugin::BioIKKinematicsPlugin, - kinematics::KinematicsBase); diff --git a/src/bio_ik-kinetic-support/src/problem.cpp b/src/bio_ik-kinetic-support/src/problem.cpp deleted file mode 100644 index ca6759e..0000000 --- a/src/bio_ik-kinetic-support/src/problem.cpp +++ /dev/null @@ -1,340 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#include "ik_base.h" - -#include -#include - -#include - -#include - -#include - -namespace bio_ik -{ - -enum class Problem::GoalType -{ - Unknown, - Position, - Orientation, - Pose, -}; - -size_t Problem::addTipLink(const moveit::core::LinkModel* link_model) -{ - if(link_tip_indices[link_model->getLinkIndex()] < 0) - { - link_tip_indices[link_model->getLinkIndex()] = tip_link_indices.size(); - tip_link_indices.push_back(link_model->getLinkIndex()); - } - return link_tip_indices[link_model->getLinkIndex()]; -} - -Problem::Problem() - : ros_params_initrd(false) -{ -} - -void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector& goals2, const BioIKKinematicsQueryOptions* options) -{ - if(robot_model != this->robot_model) - { - modelInfo = RobotInfo(robot_model); - collision_links.clear(); - collision_links.resize(robot_model->getLinkModelCount()); - } - - this->robot_model = robot_model; - this->joint_model_group = joint_model_group; - this->params = params; - - if(!ros_params_initrd) - { - ros_params_initrd = true; - dpos = params.dpos; - drot = params.drot; - dtwist = params.dtwist; - if(dpos < 0.0 || dpos >= FLT_MAX || !std::isfinite(dpos)) dpos = DBL_MAX; - if(drot < 0.0 || drot >= FLT_MAX || !std::isfinite(drot)) drot = DBL_MAX; - if(dtwist < 0.0 || dtwist >= FLT_MAX || !std::isfinite(dtwist)) dtwist = DBL_MAX; - } - - link_tip_indices.clear(); - link_tip_indices.resize(robot_model->getLinkModelCount(), -1); - tip_link_indices.clear(); - - active_variables.clear(); - auto addActiveVariable = [this, robot_model, joint_model_group, options](const std::string& name) -> ssize_t { - if(options) - { - auto& joint_name = robot_model->getJointOfVariable(name)->getName(); - for(auto& fixed_joint_name : options->fixed_joints) - { - if(fixed_joint_name == joint_name) - { - return (ssize_t)-1 - (ssize_t)robot_model->getVariableIndex(name); - } - } - } - for(size_t i = 0; i < active_variables.size(); i++) - if(name == robot_model->getVariableNames()[active_variables[i]]) return i; - for(auto& n : joint_model_group->getVariableNames()) - { - if(n == name) - { - active_variables.push_back(robot_model->getVariableIndex(name)); - return active_variables.size() - 1; - } - } - ERROR("joint variable not found", name); - }; - - goals.clear(); - secondary_goals.clear(); - for(auto& goal : goals2) - { - GoalInfo goal_info; - - goal_info.goal = goal; - - goal->describe(goal_info.goal_context); - - for(auto& link_name : goal_info.goal_context.goal_link_names_) - { - auto* link_model = robot_model->getLinkModel(link_name); - if(!link_model) ERROR("link not found", link_name); - goal_info.goal_context.goal_link_indices_.push_back(addTipLink(link_model)); - } - - for(auto& variable_name : goal_info.goal_context.goal_variable_names_) - { - goal_info.goal_context.goal_variable_indices_.push_back(addActiveVariable(variable_name)); - } - - goal_info.weight = goal_info.goal_context.goal_weight_; - goal_info.weight_sq = goal_info.weight * goal_info.weight; - - goal_info.goal_type = GoalType::Unknown; - - goal_info.frame = Frame::identity(); - goal_info.tip_index = 0; - if(goal_info.goal_context.goal_link_indices_.size()) goal_info.tip_index = goal_info.goal_context.goal_link_indices_[0]; - - if(auto* g = dynamic_cast(goal_info.goal)) - { - goal_info.goal_type = GoalType::Position; - goal_info.frame.pos = g->getPosition(); - } - - if(auto* g = dynamic_cast(goal_info.goal)) - { - goal_info.goal_type = GoalType::Orientation; - goal_info.frame.rot = g->getOrientation(); - } - - if(auto* g = dynamic_cast(goal_info.goal)) - { - goal_info.goal_type = GoalType::Pose; - goal_info.frame.pos = g->getPosition(); - goal_info.frame.rot = g->getOrientation(); - } - - goal_info.goal_context.joint_model_group_ = joint_model_group; - goal_info.goal_context.initial_guess_ = initial_guess; - - if(goal_info.goal_context.goal_secondary_) - secondary_goals.push_back(goal_info); - else - goals.push_back(goal_info); - - // if(goal_info.variable_indices.size() > temp_variables.size()) temp_variables.resize(goal_info.variable_indices.size()); - - // if(goal_info.link_indices.size() > temp_frames.size()) temp_frames.resize(goal_info.link_indices.size()); - } - - // update active variables from active subtree - joint_usage.resize(robot_model->getJointModelCount()); - for(auto& u : joint_usage) - u = 0; - for(auto tip_index : tip_link_indices) - for(auto* link_model = robot_model->getLinkModels()[tip_index]; link_model; link_model = link_model->getParentLinkModel()) - joint_usage[link_model->getParentJointModel()->getJointIndex()] = 1; - if(options) - for(auto& fixed_joint_name : options->fixed_joints) - joint_usage[robot_model->getJointModel(fixed_joint_name)->getJointIndex()] = 0; - for(auto* joint_model : joint_model_group->getActiveJointModels()) - if(joint_usage[joint_model->getJointIndex()] && !joint_model->getMimic()) - for(auto& n : joint_model->getVariableNames()) - addActiveVariable(n); - - // init weights for minimal displacement goals - { - minimal_displacement_factors.resize(active_variables.size()); - double s = 0; - for(auto ivar : active_variables) - s += modelInfo.getMaxVelocityRcp(ivar); - if(s > 0) - { - for(size_t i = 0; i < active_variables.size(); i++) - { - auto ivar = active_variables[i]; - minimal_displacement_factors[i] = modelInfo.getMaxVelocityRcp(ivar) / s; - } - } - else - { - for(size_t i = 0; i < active_variables.size(); i++) - minimal_displacement_factors[i] = 1.0 / active_variables.size(); - } - } - - initialize2(); -} - -void Problem::initialize2() -{ - for(auto* gg : {&goals, &secondary_goals}) - { - for(auto& g : *gg) - { - g.goal_context.problem_active_variables_ = active_variables; - g.goal_context.problem_tip_link_indices_ = tip_link_indices; - g.goal_context.velocity_weights_ = minimal_displacement_factors; - g.goal_context.robot_info_ = &modelInfo; - } - } -} - -double Problem::computeGoalFitness(GoalInfo& goal_info, const Frame* tip_frames, const double* active_variable_positions) -{ - goal_info.goal_context.tip_link_frames_ = tip_frames; - goal_info.goal_context.active_variable_positions_ = active_variable_positions; - return goal_info.goal->evaluate(goal_info.goal_context) * goal_info.weight_sq; -} - -double Problem::computeGoalFitness(std::vector& goals, const Frame* tip_frames, const double* active_variable_positions) -{ - double sum = 0.0; - for(auto& goal : goals) - sum += computeGoalFitness(goal, tip_frames, active_variable_positions); - return sum; -} - -bool Problem::checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) -{ - for(auto& goal : goals) - { - const auto& fa = goal.frame; - const auto& fb = tip_frames[goal.tip_index]; - - switch(goal.goal_type) - { - - case GoalType::Position: - { - if(dpos != DBL_MAX) - { - double p_dist = (fb.pos - fa.pos).length(); - if(!(p_dist <= dpos)) return false; - } - if(dtwist != DBL_MAX) - { - KDL::Frame fk_kdl, ik_kdl; - frameToKDL(fa, fk_kdl); - frameToKDL(fb, ik_kdl); - KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff.vel, KDL::Twist::Zero().vel, dtwist)) return false; - } - continue; - } - - case GoalType::Orientation: - { - if(drot != DBL_MAX) - { - double r_dist = fb.rot.angleShortestPath(fa.rot); - r_dist = r_dist * 180 / M_PI; - if(!(r_dist <= drot)) return false; - } - if(dtwist != DBL_MAX) - { - KDL::Frame fk_kdl, ik_kdl; - frameToKDL(fa, fk_kdl); - frameToKDL(fb, ik_kdl); - KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff.rot, KDL::Twist::Zero().rot, dtwist)) return false; - } - continue; - } - - case GoalType::Pose: - { - if(dpos != DBL_MAX || drot != DBL_MAX) - { - double p_dist = (fb.pos - fa.pos).length(); - double r_dist = fb.rot.angleShortestPath(fa.rot); - r_dist = r_dist * 180 / M_PI; - if(!(p_dist <= dpos)) return false; - if(!(r_dist <= drot)) return false; - } - if(dtwist != DBL_MAX) - { - KDL::Frame fk_kdl, ik_kdl; - frameToKDL(fa, fk_kdl); - frameToKDL(fb, ik_kdl); - KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M)); - if(!KDL::Equal(kdl_diff, KDL::Twist::Zero(), dtwist)) return false; - } - continue; - } - - default: - { - double dmax = DBL_MAX; - dmax = std::fmin(dmax, dpos); - dmax = std::fmin(dmax, dtwist); - double d = computeGoalFitness(goal, tip_frames.data(), active_variable_positions); - if(!(d < dmax * dmax)) return false; - } - } - } - - // LOG("checkSolutionActiveVariables true"); - - return true; -} -} diff --git a/src/bio_ik-kinetic-support/src/problem.h b/src/bio_ik-kinetic-support/src/problem.h deleted file mode 100644 index da8d260..0000000 --- a/src/bio_ik-kinetic-support/src/problem.h +++ /dev/null @@ -1,142 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include "utils.h" -#include - -#include - -#include - -#include -#include - -#include - -namespace bio_ik -{ - -class Problem -{ -private: - bool ros_params_initrd; - std::vector joint_usage; - std::vector link_tip_indices; - std::vector minimal_displacement_factors; - std::vector joint_transmission_goal_temp, joint_transmission_goal_temp2; - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup* joint_model_group; - IKParams params; - RobotInfo modelInfo; - double dpos, drot, dtwist; - struct CollisionShape - { - std::vector vertices; - std::vector points; - std::vector polygons; - std::vector plane_normals; - std::vector plane_dis; - collision_detection::FCLGeometryConstPtr geometry; - Frame frame; - std::vector> edges; - }; - struct CollisionLink - { - bool initialized; - std::vector> shapes; - CollisionLink() - : initialized(false) - { - } - }; - std::vector collision_links; - size_t addTipLink(const moveit::core::LinkModel* link_model); - -public: - /*enum class GoalType; - struct BalanceGoalInfo - { - ssize_t tip_index; - double mass; - Vector3 center; - }; - struct GoalInfo - { - const Goal* goal; - GoalType goal_type; - size_t tip_index; - double weight; - double weight_sq; - double rotation_scale; - double rotation_scale_sq; - Frame frame; - tf::Vector3 target; - tf::Vector3 direction; - tf::Vector3 axis; - double distance; - ssize_t active_variable_index; - double variable_position; - std::vector variable_indices; - mutable size_t last_collision_vertex; - std::vector balance_goal_infos; - };*/ - enum class GoalType; - // std::vector temp_frames; - // std::vector temp_variables; - struct GoalInfo - { - const Goal* goal; - double weight_sq; - double weight; - GoalType goal_type; - size_t tip_index; - Frame frame; - GoalContext goal_context; - }; - double timeout; - std::vector initial_guess; - std::vector active_variables; - std::vector tip_link_indices; - std::vector goals; - std::vector secondary_goals; - Problem(); - void initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector& goals2, const BioIKKinematicsQueryOptions* options); - void initialize2(); - double computeGoalFitness(GoalInfo& goal, const Frame* tip_frames, const double* active_variable_positions); - double computeGoalFitness(std::vector& goals, const Frame* tip_frames, const double* active_variable_positions); - bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions); -}; -} diff --git a/src/bio_ik-kinetic-support/src/utils.h b/src/bio_ik-kinetic-support/src/utils.h deleted file mode 100644 index a8edc22..0000000 --- a/src/bio_ik-kinetic-support/src/utils.h +++ /dev/null @@ -1,521 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016-2017, Philipp Sebastian Ruppel - * 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 Willow Garage 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. - *********************************************************************/ - -#pragma once - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -//#include - -//#include -//#include - -namespace bio_ik -{ - -struct IKParams -{ - moveit::core::RobotModelConstPtr robot_model; - const moveit::core::JointModelGroup* joint_model_group; - - // IKParallel parameters - std::string solver_class_name; - bool enable_counter; - int thread_count; - - //Problem parameters - double dpos; - double drot; - double dtwist; - - // ik_evolution_1 parameters - bool opt_no_wipeout; - int population_size; - int elite_count; - bool linear_fitness; -}; - -#define ENABLE_LOG - -#define ENABLE_PROFILER - -// logging - -//#define LOG_STREAM (std::cerr << std::fixed) -//#define LOG_STREAM (std::cerr << std::scientific) -#define LOG_STREAM (std::cerr) - -template inline void vprint(std::ostream& s, const T& a) { s << a << std::endl; } -template inline void vprint(std::ostream& s, const T& a, const AA&... aa) -{ - s << a << " "; - vprint(s, aa...); -}; - -#define LOG2(...) vprint(LOG_STREAM, "ikbio ", __VA_ARGS__) - -#ifdef ENABLE_LOG -#define LOG(...) LOG2(__VA_ARGS__) -#else -#define LOG(...) -#endif - -#define LOG_VAR(v) LOG2(#v, (v)); - -//#define LOG_FNC() LOG("fun", __func__, __LINE__) -#define LOG_FNC() - -// show error and abort -// #define ERROR(...) { LOG("ERROR", __VA_ARGS__); exit(-1); } -// #define ERROR(a, ...) { LOG(a, __VA_ARGS__); LOG_STREAM.flush(); throw std::runtime_error(a); } -#define ERROR(...) \ - { \ - LOG2(__VA_ARGS__); \ - LOG_STREAM.flush(); \ - std::stringstream ss; \ - vprint(ss, __VA_ARGS__); \ - throw std::runtime_error(ss.str()); \ - } -// #define ERROR(...) { LOG_ALWAYS(__VA_ARGS__); std::raise(SIGINT); } - -// profiler - -#ifdef ENABLE_PROFILER - -// embeddable sampling profiler - -// profiled block or function -struct ProfilerBin -{ - const char* volatile name; // name of scope or function, also used as indicator if it is currently being executed - std::atomic counter; // only used by CounterScope / COUNTERPROFILER - ProfilerBin() - : name(0) - { - } -}; - -// allocate globally unique profiler buffer via template -template ProfilerBin* getProfilerBuffer() -{ - static std::vector buffer(10000); - return buffer.data(); -} - -// reserve profiler buffer segment for current compilation unit -template ProfilerBin* getProfilerSegment() -{ - static size_t index = 0; - return getProfilerBuffer() + (index++) * 20; -} -static ProfilerBin* profiler_segment = getProfilerSegment(); - -// identifies currently profiled thread -// null if profiler is disabled -struct ProfilerInfo -{ - void* stack_begin; - void* stack_end; -}; - -// declare globally unique profiler info via template -template ProfilerInfo& getProfilerInfo() -{ - static ProfilerInfo info; - return info; -} -static ProfilerInfo& profiler_info = getProfilerInfo(); - -// profiles a scope or function -template struct ProfilerScope -{ - __attribute__((always_inline)) inline ProfilerScope(const char* name) - { - if(profiler_info.stack_begin == 0) return; - if(this < profiler_info.stack_begin || this > profiler_info.stack_end) return; - profiler_segment[ID].name = name; - } - __attribute__((always_inline)) inline ~ProfilerScope() - { - if(profiler_info.stack_begin == 0) return; - if(this < profiler_info.stack_begin || this > profiler_info.stack_end) return; - profiler_segment[ID].name = 0; - } -}; -#define FNPROFILER() volatile ProfilerScope<__COUNTER__> _profilerscope(__func__); -#define BLOCKPROFILER(name) volatile ProfilerScope<__COUNTER__> _profilerscope(name); - -// per-thread profiling -struct ThreadScope -{ - size_t id; - __attribute__((always_inline)) inline ThreadScope(const char* name, size_t id) - : id(id) - { - if(profiler_info.stack_begin == 0) return; - profiler_segment[id].name = name; - } - __attribute__((always_inline)) inline ~ThreadScope() - { - if(profiler_info.stack_begin == 0) return; - profiler_segment[id].name = 0; - } -}; -#define THREADPROFILER(name, id) \ - static const char* _threadscope_names[] = {name "0", name "1", name "2", name "3"}; \ - volatile ThreadScope _threadscope(_threadscope_names[id], __COUNTER__ + id); \ - (__COUNTER__, __COUNTER__, __COUNTER__, __COUNTER__, __COUNTER__); - -// profiling across multiple threads -struct CounterScope -{ - size_t id; - __attribute__((always_inline)) inline CounterScope(const char* name, size_t id) - : id(id) - { - if(profiler_info.stack_begin == 0) return; - if((profiler_segment[id].counter++) == 0) profiler_segment[id].name = name; - } - __attribute__((always_inline)) inline ~CounterScope() - { - if(profiler_info.stack_begin == 0) return; - if((--profiler_segment[id].counter) == 0) profiler_segment[id].name = 0; - } -}; -#define COUNTERPROFILER(name) volatile CounterScope _counterscope(name, __COUNTER__); - -// starts profiler and periodically writes results to log -struct Profiler -{ - std::thread thread; - volatile int exit_flag; - Profiler() - { - pthread_attr_t attr; - pthread_getattr_np(pthread_self(), &attr); - void* stack_addr; - size_t stack_size; - pthread_attr_getstack(&attr, &stack_addr, &stack_size); - profiler_info.stack_begin = stack_addr; - profiler_info.stack_end = (char*)stack_addr + stack_size; - const size_t maxbin = 1000; - static std::mutex mutex; - static std::unordered_map samples; - exit_flag = 0; - std::thread t([this]() { - auto* profiler_bins = getProfilerBuffer(); - while(true) - { - for(int iter = 0; iter < 100; iter++) - { - for(int iter = 0; iter < 100; iter++) - { - int i = rand() % maxbin; - const char* p = profiler_bins[i].name; - if(p) samples[p]++; - } - if(exit_flag) break; - std::this_thread::sleep_for(std::chrono::duration(rand() % 1000)); - } - { - double thistime = ros::WallTime::now().toSec(); - static double lasttime = 0.0; - if(thistime < lasttime + 1) continue; - lasttime = thistime; - static std::vector> data; - data.clear(); - for(auto& p : samples) - data.push_back(p); - std::sort(data.begin(), data.end(), [](const std::pair& a, const std::pair& b) { return a.second > b.second; }); - LOG(""); - LOG("profiler"); - for(auto& d : data) - { - double v = d.second * 100.0 / data[0].second; - char s[32]; - sprintf(s, "%6.2f%%", v); - LOG("p", s, d.first); - } - LOG(""); - } - if(exit_flag) break; - } - }); - std::swap(thread, t); - } - ~Profiler() - { - exit_flag = true; - thread.join(); - } - static void start() { static Profiler profiler; } -}; - -#else - -#define FNPROFILER() -#define BLOCKPROFILER(name) -#define THREADPROFILER(name, id) -#define COUNTERPROFILER(name) - -struct Profiler -{ - static void start() {} -}; - -#endif - -__attribute__((always_inline)) inline double mix(double a, double b, double f) { return a * (1.0 - f) + b * f; } - -__attribute__((always_inline)) inline double clamp(double v, double lo, double hi) -{ - if(v < lo) v = lo; - if(v > hi) v = hi; - return v; -} - -__attribute__((always_inline)) inline double clamp2(double v, double lo, double hi) -{ - if(__builtin_expect(v < lo, 0)) v = lo; - if(__builtin_expect(v > hi, 0)) v = hi; - return v; -} - -__attribute__((always_inline)) inline double smoothstep(float a, float b, float v) -{ - v = clamp((v - a) / (b - a), 0.0, 1.0); - return v * v * (3.0 - 2.0 * v); -} - -__attribute__((always_inline)) inline double sign(double f) -{ - if(f < 0.0) f = -1.0; - if(f > 0.0) f = +1.0; - return f; -} - -template class linear_int_distribution -{ - std::uniform_int_distribution base; - t n; - -public: - inline linear_int_distribution(t vrange) - : n(vrange) - , base(0, vrange) - { - } - template inline t operator()(generator& g) - { - while(true) - { - t v = base(g) + base(g); - if(v < n) return n - v - 1; - } - } -}; - -struct XORShift64 -{ - uint64_t v; - -public: - XORShift64() - : v(88172645463325252ull) - { - } - __attribute__((always_inline)) inline uint64_t operator()() - { - v ^= v << 13; - v ^= v >> 7; - v ^= v << 17; - return v; - } -}; - -// class factory -// -// registering a class: -// static Factory::Class reg("Derived"); -// -// instantiation: -// Base* obj = Factory::create("Derived"); -// -// cloning and object: -// p = Factory::clone(o); -// -template class Factory -{ - typedef BASE* (*Constructor)(ARGS...); - struct ClassBase - { - std::string name; - std::type_index type; - virtual BASE* create(ARGS... args) const = 0; - virtual BASE* clone(const BASE*) const = 0; - ClassBase() - : type(typeid(void)) - { - } - }; - typedef std::set MapType; - static MapType& classes() - { - static MapType ff; - return ff; - } - -public: - template struct Class : ClassBase - { - BASE* create(ARGS... args) const { return new DERIVED(args...); } - BASE* clone(const BASE* o) const { return new DERIVED(*(const DERIVED*)o); } - Class(const std::string& name) - { - this->name = name; - this->type = typeid(DERIVED); - classes().insert(this); - } - ~Class() { classes().erase(this); } - }; - static BASE* create(const std::string& name, ARGS... args) - { - for(auto* f : classes()) - if(f->name == name) return f->create(args...); - ERROR("class not found", name); - } - template static DERIVED* clone(const DERIVED* o) - { - for(auto* f : classes()) - if(f->type == typeid(*o)) return (DERIVED*)f->clone(o); - ERROR("class not found", typeid(*o).name()); - } -}; - -// Alloctes memory properly aligned for SIMD operations -template struct aligned_allocator : public std::allocator -{ - typedef size_t size_type; - typedef ptrdiff_t difference_type; - typedef T* pointer; - typedef const T* const_pointer; - typedef T& reference; - typedef const T& const_reference; - typedef T value_type; - T* allocate(size_t s, const void* hint = 0) - { - void* p; - if(posix_memalign(&p, A, sizeof(T) * s + 64)) throw std::bad_alloc(); - return (T*)p; - } - void deallocate(T* ptr, size_t s) { free(ptr); } - template struct rebind - { - typedef aligned_allocator other; - }; -}; - -// std::vector typedef with proper memory alignment for SIMD operations -template struct aligned_vector : std::vector> -{ -}; - -// Helper class for reading structured data from ROS parameter server -class XmlRpcReader -{ - typedef XmlRpc::XmlRpcValue var; - var& v; - -public: - XmlRpcReader(var& v) - : v(v) - { - } - -private: - XmlRpcReader at(int i) { return v[i]; } - void conv(bool& r) { r = (bool)v; } - void conv(double& r) { r = (v.getType() == var::TypeInt) ? ((double)(int)v) : ((double)v); } - void conv(tf::Vector3& r) - { - double x, y, z; - at(0).conv(x); - at(1).conv(y); - at(2).conv(z); - r = tf::Vector3(x, y, z); - } - void conv(tf::Quaternion& r) - { - double x, y, z, w; - at(0).conv(x); - at(1).conv(y); - at(2).conv(z); - at(3).conv(w); - r = tf::Quaternion(x, y, z, w).normalized(); - } - void conv(std::string& r) { r = (std::string)v; } - -public: - template void param(const char* key, T& r) - { - if(!v.hasMember(key)) return; - try - { - XmlRpcReader(v[key]).conv(r); - } - catch(const XmlRpc::XmlRpcException& e) - { - LOG(key); - throw; - } - } -}; -}