Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Eliminato il supporto a bio-ik, si potrà reinserire in futuro

Browse Source
This commit is contained in:
Emanuele
2019-10-21 10:39:21 +02:00
parent d4b246e2b3
commit c43e9cd470
26 changed files with 0 additions and 8307 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
src/bio_ik-kinetic-support/CMakeLists.txt
View File

@@ -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($<$<CONFIG:Release>:-O3>)
add_compile_options($<$<CONFIG:Release>:-ftree-vectorize>)
add_compile_options($<$<CONFIG:Release>:-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})

393
src/bio_ik-kinetic-support/README.md
View File

@@ -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<std::string>(), // 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

4
src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml
View File

@@ -1,4 +0,0 @@
<library path="lib/libbio_ik">
<class name="bio_ik/BioIKKinematicsPlugin" type="bio_ik_kinematics_plugin::BioIKKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
</class>
</library>

BIN
src/bio_ik-kinetic-support/doc/goals.pdf
View File

Binary file not shown.

BIN
src/bio_ik-kinetic-support/doc/pr2_vt_0.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 115 KiB

BIN
src/bio_ik-kinetic-support/doc/solvers.pdf
View File

Binary file not shown.

50
src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h
View File

@@ -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 <functional>
#include <memory>
#include <string>
#include <vector>
#include <tf/tf.h>
#include "goal.h"
#include "goal_types.h"
namespace bio_ik
{
}

258
src/bio_ik-kinetic-support/include/bio_ik/frame.h
View File

@@ -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 <vector>
#include <Eigen/Dense>
#include <tf_conversions/tf_kdl.h>
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 <size_t i> 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 <size_t i> const Frame Frame::IdentityFrameTemplate<i>::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;
}
}

130
src/bio_ik-kinetic-support/include/bio_ik/goal.h
View File

@@ -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 <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
namespace bio_ik
{
class RobotInfo;
class GoalContext
{
protected:
const double* active_variable_positions_;
const Frame* tip_link_frames_;
std::vector<ssize_t> goal_variable_indices_;
std::vector<size_t> goal_link_indices_;
bool goal_secondary_;
std::vector<std::string> goal_link_names_, goal_variable_names_;
double goal_weight_;
const moveit::core::JointModelGroup* joint_model_group_;
std::vector<size_t> problem_active_variables_;
std::vector<size_t> problem_tip_link_indices_;
std::vector<double> initial_guess_;
std::vector<double> velocity_weights_;
const RobotInfo* robot_info_;
mutable std::vector<double> 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<double>& 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<std::unique_ptr<Goal>> goals;
std::vector<std::string> fixed_joints;
bool replace;
mutable double solution_fitness;
BioIKKinematicsQueryOptions();
~BioIKKinematicsQueryOptions();
};
}

661
src/bio_ik-kinetic-support/include/bio_ik/goal_types.h
View File

@@ -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 <geometric_shapes/shapes.h>
#include <moveit/collision_detection/collision_robot.h>
#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
#include <map>
#include <unordered_set>
#include <geometric_shapes/bodies.h>
#include <geometric_shapes/shapes.h>
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<Vector3> vertices;
std::vector<fcl::Vec3f> points;
std::vector<int> polygons;
std::vector<fcl::Vec3f> plane_normals;
std::vector<double> plane_dis;
collision_detection::FCLGeometryConstPtr geometry;
Frame frame;
std::vector<std::vector<size_t>> edges;
};
struct CollisionLink
{
bool initialized;
std::vector<std::shared_ptr<CollisionShape>> shapes;
CollisionLink()
: initialized(false)
{
}
};
struct CollisionModel
{
std::vector<CollisionLink> 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<std::string> variable_names;
std::function<void(std::vector<double>&)> function;
public:
JointFunctionGoal() {}
JointFunctionGoal(const std::vector<std::string>& variable_names, const std::function<void(std::vector<double>&)>& function, double weight = 1.0, bool secondary = false)
: variable_names(variable_names)
, function(function)
{
weight_ = weight;
secondary_ = secondary;
}
void setJointVariableNames(const std::vector<std::string>& n) { variable_names = n; }
void setJointVariableFunction(const std::function<void(std::vector<double>&)>& 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<BalanceInfo> 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<double(const tf::Vector3&, const tf::Quaternion&)> function;
public:
LinkFunctionGoal() {}
LinkFunctionGoal(const std::string& link_name, const std::function<double(const tf::Vector3&, const tf::Quaternion&)>& function, double weight = 1.0)
: LinkGoalBase(link_name, weight)
, function(function)
{
}
void setLinkFunction(const std::function<double(const tf::Vector3&, const tf::Quaternion&)>& 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;
}
};
}

126
src/bio_ik-kinetic-support/include/bio_ik/robot_info.h
View File

@@ -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 <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <bio_ik/goal.h>
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<VariableInfo> variables;
std::vector<size_t> activeVariables;
std::vector<moveit::core::JointModel::JointType> 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<const moveit::core::RevoluteJointModel*>(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; }
};
}

47
src/bio_ik-kinetic-support/package.xml
View File

@@ -1,47 +0,0 @@
<?xml version="1.0"?>
<package>
<name>bio_ik</name>
<version>1.0.0</version>
<description>BioIK for ROS</description>
<license>BSD</license>
<author email="0ruppel@informatik.uni-hamburg.de">Philipp Sebastian Ruppel</author>
<maintainer email="0ruppel@informatik.uni-hamburg.de">Philipp Sebastian Ruppel</maintainer>
<maintainer email="me@v4hn.de">Michael Goerner</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>moveit_core</build_depend>
<run_depend>moveit_core</run_depend>
<build_depend>pluginlib</build_depend>
<run_depend>pluginlib</run_depend>
<build_depend>eigen</build_depend>
<run_depend>eigen</run_depend>
<build_depend>moveit_ros_planning</build_depend>
<run_depend>moveit_ros_planning</run_depend>
<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>
<build_depend>tf</build_depend>
<run_depend>tf</run_depend>
<build_depend>tf_conversions</build_depend>
<run_depend>tf_conversions</run_depend>
<build_depend>eigen_conversions</build_depend>
<run_depend>eigen_conversions</run_depend>
<test_depend>rosunit</test_depend>
<export>
<moveit_core plugin="${prefix}/bio_ik_kinematics_description.xml"/>
</export>
</package>

1503
src/bio_ik-kinetic-support/src/forward_kinematics.h
View File

File diff suppressed because it is too large Load Diff

269
src/bio_ik-kinetic-support/src/goal_types.cpp
View File

@@ -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 <bio_ik/goal_types.h>
#include <geometric_shapes/bodies.h>
#include <geometric_shapes/shapes.h>
#include <mutex>
namespace bio_ik
{
void TouchGoal::describe(GoalContext& context) const
{
LinkGoalBase::describe(context);
auto* robot_model = &context.getRobotModel();
{
static std::map<const moveit::core::RobotModel*, CollisionModel*> 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<CollisionShape>();
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<const shapes::Mesh*>(shape))
{
struct : bodies::ConvexMesh
{
std::vector<fcl::Vec3f> points;
std::vector<int> polygons;
std::vector<fcl::Vec3f> plane_normals;
std::vector<double> plane_dis;
void init(const shapes::Shape* shape)
{
type_ = shapes::MESH;
scaled_vertices_ = 0;
{
static std::mutex mutex;
std::lock_guard<std::mutex> 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<std::unordered_set<size_t>> 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<const shapes::Mesh*>(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();
}
}

214
src/bio_ik-kinetic-support/src/ik_base.h
View File

@@ -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 <bio_ik/goal.h>
#include "forward_kinematics.h"
#include "problem.h"
#include "utils.h"
#include <bio_ik/robot_info.h>
#include <random>
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<double>(0, 1)(rng); }
inline std::size_t random_index(std::size_t s) { return std::uniform_int_distribution<size_t>(0, s - 1)(rng); }
std::normal_distribution<double> normal_distribution;
inline double random_gauss() { return normal_distribution(rng); }
inline double random(double min, double max) { return random() * (max - min) + min; }
template <class e> inline e& random_element(std::vector<e>& l) { return l[random_index(l.size())]; }
template <class e> inline const e& random_element(const std::vector<e>& l) { return l[random_index(l.size())]; }
XORShift64 _xorshift;
inline size_t fast_random_index(size_t mod) { return _xorshift() % mod; }
template <class T> inline const T& fast_random_element(const std::vector<T>& 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<double> 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<double> 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<Frame> null_tip_frames;
volatile int canceled;
virtual void step() = 0;
virtual const std::vector<double>& 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<double>& variable_positions) { return computeSecondaryFitnessActiveVariables(extractActiveVariables(variable_positions)); }
double computeFitnessActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); }
double computeFitnessActiveVariables(const aligned_vector<Frame>& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); }
double computeCombinedFitnessActiveVariables(const std::vector<Frame>& 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<Frame>& 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<Frame>& tip_frames, const double* active_variable_positions) { return problem.checkSolutionActiveVariables(tip_frames, active_variable_positions); }
bool checkSolution(const std::vector<double>& variable_positions, const std::vector<Frame>& tips) { return checkSolutionActiveVariables(tips, extractActiveVariables(variable_positions)); }
std::vector<double> temp_active_variable_positions;
double* extractActiveVariables(const std::vector<double>& 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<double>& variable_positions, const std::vector<Frame>& tip_frames) { return computeFitnessActiveVariables(tip_frames, extractActiveVariables(variable_positions)); }
double computeFitness(const std::vector<double>& variable_positions)
{
model.applyConfiguration(variable_positions);
return computeFitness(variable_positions, model.getTipFrames());
}
virtual size_t concurrency() const { return 1; }
};
typedef IKBase IKSolver;
typedef Factory<IKSolver, const IKParams&> IKFactory;
}

257
src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp
View File

@@ -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<double>
{
IKBase* ik;
std::vector<double> 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<double>& state, const TVector& x)
{
// check ik timeout
return ros::WallTime::now().toSec() < ik->problem.timeout;
}
};
*/
// problem description for cppoptlib
struct IKOptLibProblem : cppoptlib::BoundedProblem<double>
{
IKBase* ik;
std::vector<double> fk_values;
IKOptLibProblem(IKBase* ik)
: cppoptlib::BoundedProblem<double>(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<double>()(x[i]) << i); LOG((void*)h);
return ik->computeFitness(fk_values);
}
bool callback(const cppoptlib::Criteria<double>& state, const TVector& x)
{
// check ik timeout
return ros::WallTime::now().toSec() < ik->problem.timeout;
}
};
// ik solver using cppoptlib
template <class SOLVER, int reset_if_stuck, int threads> struct IKOptLib : IKBase
{
// current solution
std::vector<double> solution, best_solution, temp;
// cppoptlib solver
std::shared_ptr<SOLVER> solver;
// cppoptlib problem description
IKOptLibProblem f;
// reset flag
bool reset;
// stop criteria
cppoptlib::Criteria<double> 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<double>::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<SOLVER>();
}
const std::vector<double>& 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<bio_ik::IKOptLib<cppoptlib::t<bio_ik::IKOptLibProblem>, reset, threads>> ik_optlib_##t##reset##threads(mkoptname(n, reset, threads));
//#define IKCPPOPT(n, t) static bio_ik::IKFactory::Class<bio_ik::IKOptLib<cppoptlib::t<bio_ik::IKOptLibProblem>>> 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);

561
src/bio_ik-kinetic-support/src/ik_evolution_1.cpp
View File

@@ -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<double> genes;
std::vector<double> gradients;
double extinction;
double fitness;
};
class HeuristicErrorTree
{
size_t variable_count, tip_count;
std::vector<double> table;
std::vector<double> chain_lengths;
std::vector<std::vector<double>> chain_lengths_2;
public:
HeuristicErrorTree() {}
HeuristicErrorTree(moveit::core::RobotModelConstPtr robot_model, const std::vector<std::string>& 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<double> solution;
std::vector<Individual> population;
int populationSize, eliteCount;
std::vector<Individual*> tempPool;
std::vector<Individual> tempOffspring;
std::vector<double> 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 <class t> inline t select(const std::vector<t>& v)
{
// FNPROFILER();
linear_int_distribution<size_t> 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<double>& 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<std::string> 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<double>& 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<Frame>& 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<IKEvolution1> cIKEvolution1("bio1");
}

659
src/bio_ik-kinetic-support/src/ik_evolution_2.cpp
View File

@@ -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 <int memetic> struct IKEvolution2 : IKBase
{
struct Individual
{
aligned_vector<double> genes;
aligned_vector<double> gradients;
double fitness;
};
struct Species
{
std::vector<Individual> individuals;
double fitness;
bool improved;
};
std::vector<double> initial_guess;
std::vector<double> solution;
std::vector<double> temp_joint_variables;
double solution_fitness;
std::vector<Species> species;
std::vector<Individual> children;
std::vector<aligned_vector<Frame>> phenotypes, phenotypes2, phenotypes3;
std::vector<size_t> child_indices;
std::vector<double*> genotypes;
std::vector<Frame> phenotype;
std::vector<size_t> quaternion_genes;
aligned_vector<double> genes_min, genes_max, genes_span;
aligned_vector<double> gradient, temp;
IKEvolution2(const IKParams& p)
: IKBase(p)
{
}
#ifdef ENABLE_CPP_OPTLIB
struct OptlibProblem : cppoptlib::Problem<double>
{
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<OptlibProblem> OptlibSolver;
std::shared_ptr<OptlibSolver> optlib_solver;
std::shared_ptr<OptlibProblem> optlib_problem;
typename OptlibSolver::TVector optlib_vector;
#endif
void genesToJointVariables(const Individual& individual, std::vector<double>& 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<double>& 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<std::vector<int>> chain_mutation_masks;
std::vector<const moveit::core::LinkModel*> temp_mutation_chain;
std::vector<int> temp_chain_mutation_var_mask;
*/
// aligned_vector<double> rmask;
// create offspring and mutate
__attribute__((hot)) __attribute__((noinline))
//__attribute__((target_clones("avx2", "avx", "sse2", "default")))
//__attribute__((target("avx")))
void
reproduce(const std::vector<Individual>& 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<OptlibSolver>();
cppoptlib::Criteria<double> crit;
crit.iterations = 4;
optlib_solver->setStopCriteria(crit);
optlib_problem = std::make_shared<OptlibProblem>(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<IKEvolution2<0>> bio2("bio2");
static IKFactory::Class<IKEvolution2<'q'>> bio2_memetic("bio2_memetic");
static IKFactory::Class<IKEvolution2<'l'>> bio2_memetic_l("bio2_memetic_l");
#ifdef ENABLE_CPP_OPTLIB
static IKFactory::Class<IKEvolution2<'o'>> bio2_memetic_0("bio2_memetic_lbfgs");
#endif
}

289
src/bio_ik-kinetic-support/src/ik_gradient.cpp
View File

@@ -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 <class BASE> struct IKJacobianBase : BASE
{
using BASE::modelInfo;
using BASE::model;
using BASE::params;
using BASE::computeFitness;
using BASE::problem;
std::vector<Frame> tipObjectives;
Eigen::VectorXd tip_diffs;
Eigen::VectorXd joint_diffs;
Eigen::MatrixXd jacobian;
std::vector<Frame> 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<double>& 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 <int if_stuck, size_t threads> struct IKGradientDescent : IKBase
{
std::vector<double> 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<double>& 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<IKGradientDescent<' ', 1>> gd("gd");
static IKFactory::Class<IKGradientDescent<' ', 2>> gd_2("gd_2");
static IKFactory::Class<IKGradientDescent<' ', 4>> gd_4("gd_4");
static IKFactory::Class<IKGradientDescent<' ', 8>> gd_8("gd_8");
static IKFactory::Class<IKGradientDescent<'r', 1>> gd_r("gd_r");
static IKFactory::Class<IKGradientDescent<'r', 2>> gd_2_r("gd_r_2");
static IKFactory::Class<IKGradientDescent<'r', 4>> gd_4_r("gd_r_4");
static IKFactory::Class<IKGradientDescent<'r', 8>> gd_8_r("gd_r_8");
static IKFactory::Class<IKGradientDescent<'c', 1>> gd_c("gd_c");
static IKFactory::Class<IKGradientDescent<'c', 2>> gd_2_c("gd_c_2");
static IKFactory::Class<IKGradientDescent<'c', 4>> gd_4_c("gd_c_4");
static IKFactory::Class<IKGradientDescent<'c', 8>> gd_8_c("gd_c_8");
// pseudoinverse jacobian only
template <size_t threads> struct IKJacobian : IKJacobianBase<IKBase>
{
using IKBase::initialize;
std::vector<double> solution;
IKJacobian(const IKParams& p)
: IKJacobianBase<IKBase>(p)
{
}
void initialize(const Problem& problem)
{
IKJacobianBase<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));
}
const std::vector<double>& getSolution() const { return solution; }
void step() { optimizeJacobian(solution); }
size_t concurrency() const { return threads; }
};
static IKFactory::Class<IKJacobian<1>> jac("jac");
static IKFactory::Class<IKJacobian<2>> jac_2("jac_2");
static IKFactory::Class<IKJacobian<4>> jac_4("jac_4");
static IKFactory::Class<IKJacobian<8>> jac_8("jac_8");
}

690
src/bio_ik-kinetic-support/src/ik_neural.cpp
View File

@@ -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 <fann.h>
#include <fann_cpp.h>
#include <mutex>
#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<double> solution;
FANN::neural_net net;
std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& 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<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& 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<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& 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<unsigned int> levels = {input_count, input_count, input_count, output_count};
// std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
// std::vector<unsigned int> levels = {input_count, input_count + output_count, input_count + output_count, output_count};
// std::vector<unsigned int> levels = {input_count, 100, output_count};
std::vector<unsigned int> levels = {input_count, 50, output_count};
net.create_standard_array(levels.size(), levels.data());
size_t var_count = params.robot_model->getVariableCount();
std::vector<double> state1(var_count), state2(var_count);
std::vector<Frame> frames1, frames2;
std::vector<fann_type> inputs, outputs;
std::vector<fann_type*> 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<fann_type> 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<std::mutex> 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<double>& getSolution() const { return solution; }
std::vector<fann_type> inputs, outputs;
std::vector<Frame> 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<IKNeural> neural("neural");
struct IKNeural2 : IKBase
{
std::vector<double> solution;
FANN::neural_net net;
std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
/*static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& 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<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
{
std::vector<double> 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<double> 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<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& 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<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& 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<unsigned int> levels = {input_count, 100, 100, output_count};
// std::vector<unsigned int> levels = {input_count, input_count, input_count, output_count};
std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
// std::vector<unsigned int> levels = {input_count, input_count + output_count, input_count + output_count, output_count};
// std::vector<unsigned int> levels = {input_count, output_count};
net.create_standard_array(levels.size(), levels.data());
size_t var_count = params.robot_model->getVariableCount();
std::vector<double> state = problem.initial_guess;
std::vector<Frame> frames;
std::vector<fann_type> inputs, outputs;
std::vector<fann_type*> 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<fann_type> 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<double>& getSolution() const { return solution; }
std::vector<fann_type> inputs, outputs;
std::vector<Frame> 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<IKNeural2> neural2("neural2");
}

278
src/bio_ik-kinetic-support/src/ik_parallel.h
View File

@@ -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 <boost/thread/barrier.hpp>
namespace bio_ik
{
// executes a function in parallel on pre-allocated threads
class ParallelExecutor
{
std::function<void(size_t)> fun;
std::vector<std::thread> threads;
boost::barrier barrier;
volatile bool exit;
double best_fitness;
public:
template <class FUN>
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<std::unique_ptr<IKSolver>> solvers;
std::vector<std::vector<double>> solver_solutions;
std::vector<std::vector<double>> solver_temps;
std::vector<int> solver_success;
std::vector<double> solver_fitness;
int thread_count;
// std::vector<RobotFK_Fast> fk; // TODO: remove
double timeout;
bool success;
std::atomic<int> finished;
std::atomic<uint32_t> iteration_count;
std::vector<double> result;
std::unique_ptr<ParallelExecutor> 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<double>& getSolution() const { return result; }
};
}

137
src/bio_ik-kinetic-support/src/ik_test.cpp
View File

@@ -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<double> 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<Frame>& fa, const std::vector<Frame>& 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<aligned_vector<Frame>> fbm;
std::vector<double> 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<double>& getSolution() const { return problem.initial_guess; }
};
static IKFactory::Class<IKTest> test("test");
}

649
src/bio_ik-kinetic-support/src/kinematics_plugin.cpp
View File

@@ -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 <bio_ik/goal.h>
#include "forward_kinematics.h"
#include "ik_base.h"
#include "ik_parallel.h"
#include "problem.h"
#include "utils.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <kdl_parser/kdl_parser.hpp>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/rdf_loader/rdf_loader.h>
#include <pluginlib/class_list_macros.h>
#include <srdfdom/model.h>
#include <urdf/model.h>
#include <urdf_model/model.h>
#include <eigen_conversions/eigen_msg.h>
//#include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <atomic>
#include <mutex>
#include <random>
#include <tuple>
#include <type_traits>
#include <bio_ik/goal_types.h>
using namespace bio_ik;
// implement BioIKKinematicsQueryOptions
namespace bio_ik {
std::mutex bioIKKinematicsQueryOptionsMutex;
std::unordered_set<const void *> bioIKKinematicsQueryOptionsList;
BioIKKinematicsQueryOptions::BioIKKinematicsQueryOptions()
: replace(false), solution_fitness(0) {
std::lock_guard<std::mutex> lock(bioIKKinematicsQueryOptionsMutex);
bioIKKinematicsQueryOptionsList.insert(this);
}
BioIKKinematicsQueryOptions::~BioIKKinematicsQueryOptions() {
std::lock_guard<std::mutex> lock(bioIKKinematicsQueryOptionsMutex);
bioIKKinematicsQueryOptionsList.erase(this);
}
bool isBioIKKinematicsQueryOptions(const void *ptr) {
std::lock_guard<std::mutex> 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 <class T>
static void lookupParam(const std::string &param, T &val,
const T &default_val) {
ros::NodeHandle nodeHandle("~");
val = nodeHandle.param(param, default_val);
}
struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
std::vector<std::string> joint_names, link_names;
moveit::core::RobotModelConstPtr robot_model;
const moveit::core::JointModelGroup *joint_model_group;
mutable std::unique_ptr<IKParallel> ik;
mutable std::vector<double> state, temp;
mutable std::unique_ptr<moveit::core::RobotState> temp_state;
mutable std::vector<Frame> tipFrames;
RobotInfo robot_info;
bool enable_profiler;
BioIKKinematicsPlugin() { enable_profiler = false; }
virtual const std::vector<std::string> &getJointNames() const {
LOG_FNC();
return joint_names;
}
virtual const std::vector<std::string> &getLinkNames() const {
LOG_FNC();
return link_names;
}
virtual bool getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const {
LOG_FNC();
return false;
}
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options =
kinematics::KinematicsQueryOptions()) const {
LOG_FNC();
return false;
}
std::vector<Eigen::Affine3d> tip_reference_frames;
mutable std::vector<std::unique_ptr<Goal>> default_goals;
mutable std::vector<const bio_ik::Goal *> all_goals;
IKParams ikparams;
mutable Problem problem;
static moveit::core::RobotModelConstPtr
loadRobotModel(const std::string &robot_description) {
static std::map<std::string, moveit::core::RobotModelConstPtr>
robot_model_cache;
static std::mutex cache_mutex;
std::lock_guard<std::mutex> 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<std::string> 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<std::string> &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<double> &ik_seed_state, double timeout,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options =
kinematics::KinematicsQueryOptions()) const {
LOG_FNC();
return searchPositionIK(std::vector<geometry_msgs::Pose>{ik_pose},
ik_seed_state, timeout, std::vector<double>(),
solution, IKCallbackFn(), error_code, options);
}
virtual bool
searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state, double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options =
kinematics::KinematicsQueryOptions()) const {
LOG_FNC();
return searchPositionIK(std::vector<geometry_msgs::Pose>{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<double> &ik_seed_state, double timeout,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options =
kinematics::KinematicsQueryOptions()) const {
LOG_FNC();
return searchPositionIK(std::vector<geometry_msgs::Pose>{ik_pose},
ik_seed_state, timeout, std::vector<double>(),
solution, solution_callback, error_code, options);
}
virtual bool
searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state, double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options =
kinematics::KinematicsQueryOptions()) const {
LOG_FNC();
return searchPositionIK(std::vector<geometry_msgs::Pose>{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<geometry_msgs::Pose> &ik_poses,
const std::vector<double> &ik_seed_state, double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &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<std::unique_ptr<Goal>> 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);

340
src/bio_ik-kinetic-support/src/problem.cpp
View File

@@ -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 <geometric_shapes/bodies.h>
#include <geometric_shapes/shapes.h>
#include <unordered_set>
#include <mutex>
#include <bio_ik/goal_types.h>
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<const Goal*>& 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<const PositionGoal*>(goal_info.goal))
{
goal_info.goal_type = GoalType::Position;
goal_info.frame.pos = g->getPosition();
}
if(auto* g = dynamic_cast<const OrientationGoal*>(goal_info.goal))
{
goal_info.goal_type = GoalType::Orientation;
goal_info.frame.rot = g->getOrientation();
}
if(auto* g = dynamic_cast<const PoseGoal*>(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<GoalInfo>& 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<Frame>& 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;
}
}

142
src/bio_ik-kinetic-support/src/problem.h
View File

@@ -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 <vector>
#include <bio_ik/robot_info.h>
#include <geometric_shapes/shapes.h>
#include <moveit/collision_detection/collision_robot.h>
#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
#include <bio_ik/goal.h>
namespace bio_ik
{
class Problem
{
private:
bool ros_params_initrd;
std::vector<int> joint_usage;
std::vector<ssize_t> link_tip_indices;
std::vector<double> minimal_displacement_factors;
std::vector<double> 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<Vector3> vertices;
std::vector<fcl::Vec3f> points;
std::vector<int> polygons;
std::vector<fcl::Vec3f> plane_normals;
std::vector<double> plane_dis;
collision_detection::FCLGeometryConstPtr geometry;
Frame frame;
std::vector<std::vector<size_t>> edges;
};
struct CollisionLink
{
bool initialized;
std::vector<std::shared_ptr<CollisionShape>> shapes;
CollisionLink()
: initialized(false)
{
}
};
std::vector<CollisionLink> 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<ssize_t> variable_indices;
mutable size_t last_collision_vertex;
std::vector<BalanceGoalInfo> balance_goal_infos;
};*/
enum class GoalType;
// std::vector<const Frame*> temp_frames;
// std::vector<double> 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<double> initial_guess;
std::vector<size_t> active_variables;
std::vector<size_t> tip_link_indices;
std::vector<GoalInfo> goals;
std::vector<GoalInfo> secondary_goals;
Problem();
void initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector<const Goal*>& goals2, const BioIKKinematicsQueryOptions* options);
void initialize2();
double computeGoalFitness(GoalInfo& goal, const Frame* tip_frames, const double* active_variable_positions);
double computeGoalFitness(std::vector<GoalInfo>& goals, const Frame* tip_frames, const double* active_variable_positions);
bool checkSolutionActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions);
};
}

521
src/bio_ik-kinetic-support/src/utils.h
View File

@@ -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 <csignal>
#include <iostream>
#include <ros/ros.h>
#include <atomic>
#include <mutex>
#include <thread>
#include <typeindex>
#include <unordered_map>
#include <unordered_set>
#include <malloc.h>
#include <stdlib.h>
#include <tf_conversions/tf_kdl.h>
#include <XmlRpcException.h>
//#include <link.h>
//#include <boost/align/aligned_allocator.hpp>
//#include <Eigen/Eigen>
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 <class T> inline void vprint(std::ostream& s, const T& a) { s << a << std::endl; }
template <class T, class... AA> 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<int> counter; // only used by CounterScope / COUNTERPROFILER
ProfilerBin()
: name(0)
{
}
};
// allocate globally unique profiler buffer via template
template <class force_weak_linker_symbol = void> ProfilerBin* getProfilerBuffer()
{
static std::vector<ProfilerBin> buffer(10000);
return buffer.data();
}
// reserve profiler buffer segment for current compilation unit
template <class force_weak_linker_symbol = void> 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 <class force_weak_linker_symbol = void> ProfilerInfo& getProfilerInfo()
{
static ProfilerInfo info;
return info;
}
static ProfilerInfo& profiler_info = getProfilerInfo();
// profiles a scope or function
template <size_t ID> 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<const char*, size_t> 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<size_t, std::micro>(rand() % 1000));
}
{
double thistime = ros::WallTime::now().toSec();
static double lasttime = 0.0;
if(thistime < lasttime + 1) continue;
lasttime = thistime;
static std::vector<std::pair<const char*, size_t>> data;
data.clear();
for(auto& p : samples)
data.push_back(p);
std::sort(data.begin(), data.end(), [](const std::pair<const char*, size_t>& a, const std::pair<const char*, size_t>& 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 t> class linear_int_distribution
{
std::uniform_int_distribution<t> base;
t n;
public:
inline linear_int_distribution(t vrange)
: n(vrange)
, base(0, vrange)
{
}
template <class generator> 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<Base>::Class<Derived> reg("Derived");
//
// instantiation:
// Base* obj = Factory<Base>::create("Derived");
//
// cloning and object:
// p = Factory<Base>::clone(o);
//
template <class BASE, class... ARGS> 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<ClassBase*> MapType;
static MapType& classes()
{
static MapType ff;
return ff;
}
public:
template <class DERIVED> 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 <class DERIVED> 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 <class T, size_t A> struct aligned_allocator : public std::allocator<T>
{
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 <class U> struct rebind
{
typedef aligned_allocator<U, A> other;
};
};
// std::vector typedef with proper memory alignment for SIMD operations
template <class T> struct aligned_vector : std::vector<T, aligned_allocator<T, 32>>
{
};
// 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 <class T> 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;
}
}
};
}