bio_ik
1.0.0
該儲存庫提供了 BSD 許可的各種最佳化方法的獨立實現，以有效解決廣義逆運動學問題。
整個模組由 Philipp Ruppel 作為其碩士論文的一部分實施。
對於最初在 Unity 商店中出售的原始“BioIK”演算法的基於 C++ 的重新實現，您可以使用非預設模式bio1 。預設模式bio2_memetic與此實現不共享任何程式碼，事實證明它在成功率、精度和效率方面優於它，並且實際上可用於精確的機器人應用[4]。
您將需要 ROS 版本 Indigo 或更高版本 (wiki.ros.org)。該軟體是在具有 ROS Kinetic 的 Ubuntu Linux 16.04 LTS 上開發的，但也在帶有 ROS Indigo 的 Ubuntu Linux 14.04 LTS 上進行了測試。較新版本的 ROS 應該可以工作，但可能需要一些調整。請參閱下面的版本特定說明。
下載bio_ik包並解壓縮到catkin工作區。
執行catkin_make來編譯您的工作區：
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
配置 Moveit 以使用 bio_ik 作為運動學解算器（請參閱下一節）。
使用 Moveit 規劃和執行運動，或使用您自己的程式碼與move_group節點一起移動您的機器人。
與往常一樣，公共 API 在bio_ik包的公共頭檔中指定，位於include/bio_ik子目錄中；包括一些私有頭檔的原始碼位於src子目錄中。
為了易於使用並與現有程式碼相容，bio_ik 演算法被封裝為 Moveit 運動學插件。因此，bio_ik 可以用作預設的基於 Orocos/KDL 的 IK 解算器的直接替代。給定末端執行器的名稱和 6-DOF 目標位姿，bio_ik 將搜尋達到給定目標的有效機器人關節配置。
在我們的測試中（見下文），無論是在成功率還是求解時間方面，bio_ik 的表現通常都優於 Orocos [1] 求解器，並且與 trac-ik [2] 具有競爭力。 Bio_ik 演算法也可以用於像機器人蛇這樣的高自由度系統，它會自動收斂到低自由度手臂的最佳近似解，其中某些目標位姿無法精確到達。
雖然您可以手動編寫 Moveit 配置文件，但最簡單的方法是為您的機器人執行 Moveit 設定助手，然後在配置末端執行器時選擇 bio_ik 作為 IK 解算器。配置完成後，可以使用標準 Moveit API 呼叫解算器，或使用 MotionPlanning GUI 外掛程式從 rviz 互動使用。
確保您的機器人有 URDF（或 xacro）模型。
執行 moveit 設定助手來建立 Moveit 設定檔：
rosrun moveit_setup_assistant moveit_setup_assistant
設定助手會自動搜尋工作區中所有可用的 IK 解算器外掛程式。因此，您只需從每個末端執行器的下拉清單中選擇“bio_ik”作為 IK 解算器，然後配置運動學參數，即預設位置精度（米）和逾時（以秒為單位）。對於典型的 6-DOF 或 7-DOF 臂，0.001 m（或更小）的精度和 1 毫秒的超時應該可以。更複雜的機器人可能需要更長的超時。
從設定助手產生 moveit 設定檔。當然，您也可以使用您喜歡的文字編輯器編輯config/kinematics.yaml設定檔。例如，PR2 機器人的配置可能如下所示：
# 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
對於第一個測試，執行 Moveit 建立的簡報啟動。 rviz 運作後，啟用運動規劃插件，然後選擇機器人的末端執行器之一。 Rviz 應顯示所選末端執行器的 6-D（位置和方向）互動式標記。移動互動式標記並觀看 bio_ik 計算機器人的姿勢。
如果您也安裝了bio_ik演示（請參閱下文），您應該可以執行預先定義的演示之一：
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
現在，您可以使用標準 Moveit API 從 C/C++ 和 Python 程式中使用 bio_ik。要在 C++ 中明確請求 IK 解決方案：
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
);
對於許多機器人應用來說，指定的不僅僅是一個末端執行器姿勢是至關重要的。典型例子包括
在bio_ik中，此類任務被指定為多個單獨目標的組合。
然後，演算法嘗試透過最小化根據加權的各個目標構建的二次誤差函數來找到同時滿足所有給定目標的機器人配置。雖然目前的 Moveit API 不直接支援多目標任務，但它提供了 KinematicQueryOptions 類別。因此，bio_ik 只是提供一組預先定義的運動目標，而使用者指定的目標的組合會透過 Moveit 傳遞到 IK 解算器。 Moveit 中不需要更改 API，但現在使用 IK 解算器包含透過 KinematicQueryOptions 傳遞加權目標。預定目標包括：
要解決機器人的運動問題，現在的訣竅是建立適當的各個目標組合。
在下面的範例中，我們要使用 PR2 機器人的左右夾具抓取並緩慢轉動閥輪：
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);
我們也設定了一些次要目標。首先，我們希望 PR2 的頭部位於閥門的中心。其次，如果可能的話，我們希望避免所有關節的關節限制。第三，我們希望 IK 解決方案盡可能接近先前的關節配置，這意味著小而高效的運動。這是透過添加 MinimalDisplacementGoal 來處理的。第四，我們要避免軀幹抬起動作，這在 PR2 上非常緩慢。所有這些都很容易指定：
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(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
}
當您執行程式碼時，PR2 將到達閥輪並轉動它。每隔一段時間，它就無法以其當前的臂配置到達閥門，並且會重新抓住輪子。
更多範例請參閱 [3] 和 [4]。
BioIK 的開發旨在有效地為具有多個目標和局部最小值的非凸逆運動學問題找到良好的解決方案。然而，對於某些應用程序，這可能會導致不直觀的結果。如果給定的 IK 問題有多種可能的解決方案，且使用者沒有明確指定選擇哪一個，則可以從所有有效解決方案的集合中隨機選擇一個結果。當增量追蹤笛卡爾路徑時，這可能會導致不必要的跳躍、晃動等。
BioIK 提供了許多不同的求解器，包括全域最佳化器和局部最佳化器。預設情況下，BioIK 使用迷因全域最佳化器 ( bio2_memetic )。可以透過在 MoveIt 機器人配置的kinematics.yaml檔案中設定mode參數來選擇不同的解算器類別。
例子：
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
目前可用的本地優化器：
gd, gd_2, gd_4, gd_8
gd_r, gd_r_2, gd_r_4, gd_r_8
gd_c, gd_c_2, gd_c_4, gd_c_8
jac, jac_2, jac_4, jac_8
命名約定： <solver type>_[<variant>_]<number of threads>
筆記：
gd_*求解器支援任意目標類型。jac_*解算器僅支援姿勢目標，但理論上在某些情況下可能更穩定。gd_c變體通常優於其他本地求解器。gd_c 、 gd或jac ）。您可以透過正規化強制全域最佳化器傳回局部最小值。
對於增量機器人運動（也稱為慢跑）的特定情況，最簡單的解決方案是指定PoseGoal和特殊的RegularizationGoal ，它試圖使關節空間 IK 解決方案盡可能接近給定的機器人種子配置。通常，您會為PoseGoal使用較高的權重，並為正規化器使用較小的權重。
您也可以添加MinimalDisplacementGoal而不是RegularizationGoal 。這兩個目標都試圖保持 IK 解決方案接近當前/種子機器人狀態，但在快速和慢速機器人關節的處理方面略有不同（例如，PR2 具有快速手臂關節和相當慢的軀幹提升關節）。您可能想同時使用這兩個目標，看看哪一個更符合您的需求。
7 自由度手臂上的某些工業機器人控制器的行為就像在額外關節的 6 自由度手臂上工作一樣。通常，可以指定額外關節的值，然後搜尋剩餘六個關節的 IK 解決方案。在 bio_ik 中，可以透過將末端執行器的PoseGoal與一個（任一個）機器人關節的JointPositionGoal結合來實現這一行為。
另一個有用的技巧是嘗試保持機器人關節居中，因為這將允許機器人（關節）在兩個方向上移動。只需將PoseGoal與CenterJointsGoal以及可選的RegularizationGaol結合起來即可。
您也可以將正規化與本機gd_*求解器結合。
bio_ik 求解器基於模因演算法，該演算法將基於梯度的最佳化與遺傳和粒子群最佳化相結合。
在內部，所有機器人關節值的向量用於編碼不同的中間解（遺傳演算法的基因型）。在最佳化過程中，請務必根據有效的關節下限和上限檢查關節值，以便僅產生有效的機器人配置。
為了計算個體的適應度，需要計算所有給定個體目標的累積誤差。任何誤差為零的個體都是IK問題的精確解，而誤差較小的個體則對應於近似解。
根據個體的適應度對個體進行排序，並在最好的少數配置上嘗試基於梯度的優化，從而在許多問題上實現快速收斂和良好性能。如果基於梯度的最佳化沒有找到解決方案，則透過一組突變和重組算子創建新個體，從而產生良好的搜尋空間探索。
有關更多詳細信息，請參閱[3]和[4]。有關電玩角色動畫的早期演化演算法的深入解釋，請參閱[5]和[6]。
我們在許多不同的機器人手臂上測試了 bio_ik，兩者都使用傳統的單端執行器 API 和基於 KinematicsQueryOptions 的高級多端執行器 API。
一個簡單的自我測試包括產生隨機有效的機器人配置、運行正向運動學以計算最終的末端執行器姿勢，以及查詢 IK 插件以找到合適的機器人關節配置。然後，透過再次執行正向運動學並檢查產生的 IK 解決方案的末端執行器姿勢是否與目標姿勢相符來檢查是否成功。這種方法可以輕鬆運行數千或數百萬個隨機姿勢，對機器人的整個工作空間進行採樣，並允許快速生成所選 IK 求解器的成功率和求解時間估計。
當然，運行測試需要安裝相應的機器人模型，並且添加許多依賴項。因此，那些測試並不包含在標準bio_ik套件中，而是單獨打包的。
為了方便起見，我們提供了pr2_bioik_moveit套件，其中還包括一些 PR2 服務機器人的 bio_ik 演示。這些只是運動學示範；當然，您也可以嘗試在真實的機器人（如果您有的話）或 Gazebo 模擬器（如果您安裝了 Gazebo）上執行演示。
只需將 PR2 描述套件（在pr2_common內）和pr2_bioik_moveit套件克隆到您的 catkin 工作區中：
roscd
cd src
git clone https://github.com/PR2/pr2_common.git
git clone https://github.com/TAMS-Group/bioik_pr2.git
catkin_make
對於 FK-IK-FK 性能測試，請執行
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
Orocos 運動學和動力學，http://www.orocos.org
P. Beeson 和 B. Ames， TRAC-IK：用於改進通用逆運動學求解的開源庫，IEEE RAS 類人機器人會議論文集，韓國首爾，2015 年 11 月。
Philipp Ruppel、Norman Hendrich、Sebastian Starke、Jianwei 張，指定全身運動和多目標操縱任務的成本函數，IEEE 國際機器人與自動化會議，(ICRA-2018)，澳洲布里斯班。 DOI：10.1109/ICRA.2018.8460799
Philipp Ruppel， ROS 中進化逆運動學的性能優化和實現，碩士論文，漢堡大學，2017 年 PDF
Sebastian Starke、Norman Hendrich、Jianwei 張，即時關節運動的模因進化演算法，IEEE Intl。進化計算大會 (CEC-2017)，p.2437-2479，2017 年 6 月 4-8 日，西班牙聖塞巴斯蒂安。 DOI：10.1109/CEC.2017.7969605
Sebastian Starke、Norman Hendrich、Dennis Krupke、Jianwei 張，高關節和人形機器人逆運動學的多目標進化優化，IEEE Intl。智慧機器人與系統會議 (IROS-2017)，2017 年 9 月 24-28 日，加拿大溫哥華
