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 提供了许多不同的求解器，包括全局优化器和局部优化器。默认情况下，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 日，加拿大温哥华
