frankx 0.2.0

Franka Emika 机器人的高级运动库

Frankx 是用于 Franka Emika 机器人的高级运动库（C++ 和 Python）。它在libfranka周围添加了一个 Python 包装器，同时用更高级别的运动命令替换了必要的实时编程。由于 frankx 专注于简化实时轨迹生成，它允许机器人对不可预见的事件做出反应。

安装

要开始将 frankx 与 Python 和 libfranka 0.9.0一起使用，只需通过以下方式安装即可

pip install frankx

Frankx 基于libfranka，用于转换计算的Eigen和用于 Python 绑定的pybind11 。Frankx 使用Ruckig社区版进行在线轨迹生成 (OTG)。由于 Franka 对加速度的不连续性非常敏感，因此它要求对所有运动进行约束加加速度。安装依赖项后（可以在下面找到确切的版本），您可以通过以下方式构建和安装 frankx

git clone --recurse-submodules git@github.com:pantor/frankx.git
cd frankx
mkdir -p build
cd build
cmake -DBUILD_TYPE=Release ..
make
make install

add_subdirectory(frankx)要使用 frankx，您还可以通过然后将其作为子项目包含在您的父 CMake 中target_link_libraries(<target> libfrankx)。如果你只需要 Python 模块，你可以通过安装 frankx

pip install .

通过调整 Python 路径，确保可以从 Python 中找到构建的库。

使用 Docker

为了在 Docker 中使用 frankx，我们提供了一个Dockerfile，您目前需要自己构建它：

git clone https://github.com/pantor/frankx.git
cd frankx/
docker build -t pantor/frankx --build-arg libfranka_version=0.7.0 -f docker/Dockerfile .

要使用默认版本 (v.0.7.0) 之外的另一个版本的 libfranka，只需更改构建参数即可。然后，简单地运行容器：

docker run -it --rm --network=host --privileged pantor/frankx

容器需要访问主机网络和提升的用户权限，以允许 docker 用户设置在其中运行的进程的 RT 功能。

教程

Frankx 带有 C++ 和 Python API，它们仅在实时功能方面有所不同。我们将同时介绍这两种语言。在您的 C++ 项目中，只需包含include <frankx/frankx.hpp>并链接该库。对于 Python，只需import frankx. 作为第一个示例，简单的机器人运动只需要四行代码。

#include <frankx/frankx.hpp>
using namespace frankx;

// Connect to the robot with the FCI IP address
Robot robot("172.16.0.2");

// Reduce velocity and acceleration of the robot
robot.setDynamicRel(0.05);

// Move the end-effector 20cm in positive x-direction
auto motion = LinearRelativeMotion(Affine(0.2, 0.0, 0.0));

// Finally move the robot
robot.move(motion);

Python中对应的程序是

from frankx import Affine, LinearRelativeMotion, Robot

robot = Robot("172.16.0.2")
robot.set_dynamic_rel(0.05)

motion = LinearRelativeMotion(Affine(0.2, 0.0, 0.0))
robot.move(motion)

此外，我们将介绍几何计算方法，根据不同的运动类型移动机器人，如何实现实时反应和实时改变航点以及控制夹具。

几何学

frankx::Affine是Eigen::Affine3d周围的薄包装。它用于笛卡尔姿势、帧和变换。Frankx 简化了欧拉角的使用（默认 ZYX 约定）。

// Initiliaze a transformation with an (x, y, z, a=0.0, b=0.0, c=0.0) translation
Affine z_translation = Affine(0.0, 0.0, 0.5);

// Define a rotation transformation using the (x, y, z, a, b, c) parameter list
Affine z_rotation = Affine(0.0, 0.0, 0.0, M_PI / 3, 0.0, 0.0);

// Make use of the wonderful Eigen library
auto combined_transformation = z_translation * z_rotation;

// Get the Euler angles (a, b, c) in a vector representation
Eigen::Vector3d euler_angles = combined_transformation.angles();

// Get the vector representation (x, y, z, a, b, c) of an affine transformation
frankx::Vector6d pose = combined_transformation.vector();

在所有情况下，距离以 [m] 为单位，旋转以 [rad] 为单位。此外，还有几个帮助函数用于在 Eigen 和 libfranka 的 std::array 对象之间进行转换。在python中，这转化为

z_translation = Affine(0.0, 0.0, 0.5)
z_rotation = Affine(0.0, 0.0, 0.0, math.pi / 3, 0.0, 0.0)
combined_transformation = z_translation * z_rotation

# These two are now numpy arrays
euler_angles = combined_transformation.angles()
pose = combined_transformation.vector()

由于轨迹生成在欧拉空间中工作，请确保您的工作点周围有连续的欧拉角。您可以通过将法兰设置为末端执行器变换来调整此设置setEE(...)。

机器人

我们为 Python 包装了大部分 libfanka API（包括 RobotState 或 ErrorMessage）。此外，我们添加了方法来调整机器人的动力学以适应所有运动。该rel名称表示这是机器人最大约束的一个因素。

robot = Robot("172.16.0.2")

# Recover from errors
robot.recover_from_errors()

# Set velocity, acceleration and jerk to 5% of the maximum
robot.set_dynamic_rel(0.05)

# Alternatively, you can define each constraint individually
robot.velocity_rel = 0.2
robot.acceleration_rel = 0.1
robot.jerk_rel = 0.01

# Get the current pose
current_pose = robot.current_pose()

运动类型

Frankx 定义了五种不同的运动类型。在 python 中，您可以按如下方式使用它们：

# A point-to-point motion in the joint space
m1 = JointMotion([-1.81194, 1.17910, 1.75710, -2.1416, -1.14336, 1.63304, -0.43217])

# A linear motion in cartesian space
m2 = LinearMotion(Affine(0.2, -0.4, 0.3, math.pi / 2, 0.0, 0.0))
m3 = LinearMotion(Affine(0.2, -0.4, 0.3, math.pi / 2, 0.0, 0.0), elbow=1.7)  # With target elbow angle

# A linear motion in cartesian space relative to the initial position
m4 = LinearRelativeMotion(Affine(0.0, 0.1, 0.0))

# A more complex motion by defining multiple waypoints
m5 = WaypointMotion([
  Waypoint(Affine(0.2, -0.4, 0.2, 0.3, 0.2, 0.1)),
  # The following waypoint is relative to the prior one
  Waypoint(Affine(0.0, 0.1, 0.0), Waypoint.ReferenceType.Relative)
])

# Hold the position for [s]
m6 = PositionHold(5.0)

真正的机器人可以通过对机器人应用运动来移动move：

robot.move(m1)
robot.move(m2)

# To use a given frame relative to the end effector
camera_frame = Affine(0.1, 0.0, 0.1)
robot.move(camera_frame, m3)

# To change the dynamics of the motion, use MotionData
data = MotionData(0.2)  # Using a dynamic_rel of 0.2 (eventually multiplied with robot.dynamic_rel)
robot.move(m4, data)

使用 MotionData，您可以调整特定运动的动力学（速度、加速度和加加速度）。

data.velocity_rel = 1.0
data.jerk_rel = 0.2

实时反应

通过向运动数据添加反应，机器人可以对不可预见的事件做出反应。在 Python API 中，您可以通过使用机器人的值与给定阈值之间的比较来定义条件。如果超过阈值，则触发反应。目前正在实施以下比较

reaction_motion = LinearRelativeMotion(Affine(0.0, 0.0, 0.01))  # Move up for 1cm

# Stop motion if the overall force is greater than 30N
d1 = MotionData().with_reaction(Reaction(Measure.ForceXYZNorm() > 30.0))

# Apply reaction motion if the force in negative z-direction is greater than 10N
d2 = MotionData().with_reaction(Reaction(Measure.ForceZ() < -10.0), reaction_motion)

# Stop motion if its duration is above 30s
d3 = MotionData().with_reaction(Reaction(Measure.Time() >= 30.0))

robot.move(m2, d2)

# Check if the reaction was triggered
if d2.has_fired:
  robot.recover_from_errors()
  print('Force exceeded 10N!')

一旦反应被触发，它将被进一步忽略。在 C++ 中，您还可以使用 lambda 来定义更复杂的行为：

// Stop motion if force is over 10N
auto data = MotionData()
  .withReaction({
    Measure::ForceXYZNorm() > 10.0  // [N]
  })
  .withReaction({
    [](const franka::RobotState& state, double time) {
      return (state.current_errors.self_collision_avoidance_violation);
    }
  });

// Hold position for 5s
robot.move(PositionHold(5.0), data); // [s]
// e.g. combined with a PositionHold, the robot continues its program after pushing the end effector.

实时航点运动

当机器人在后台线程中移动时，您可以实时更改航点。

robot.moveAsync(motion_hold);

// Wait for key input from user
std::cin.get();

motion_hold.setNextWaypoint(Waypoint(Affine(0.0, 0.0, 0.1), Waypoint::ReferenceType::Relative);

夹持器

在frankx::Gripper类中，可以设置默认夹爪力和夹爪速度。然后，除了 libfranka 命令之外，还可以使用以下辅助方法：

auto gripper = Gripper("172.16.0.2");

// These are the default values
gripper.gripper_speed = 0.02; // [m/s]
gripper.gripper_force = 20.0; // [N]

// Preshape gripper before grasp, use the given speed
gripper.move(50.0); // [mm]

// Grasp an object of unknown width
is_grasping = gripper.clamp();

// Do something
is_grasping &= gripper.isGrasping();

// Release an object and move to a given distance
if (is_grasping) {
  gripper.release(50.0);
}

对于 Gripper 类，Python API 应该非常简单。

运动学

Frankx 包括一个基本的、非实时的正向和反向运动学。

# Some initial joint configuration
q = [-1.45549, 1.15401, 1.50061, -2.30909, -1.3141, 1.9391, 0.02815]

# Calculate the forward kinematics
x = Affine(Kinematics.forward(q))
print('Current end effector position: ', x)

# Define new target position
x_new = Affine(x=0.1, y=0.0, z=0.0) * x

# Franka has 7 DoFs, so what to do with the remaining Null space?
null_space = NullSpaceHandling(2, 1.4) # Set elbow joint to 1.4

# Inverse kinematic with target, initial joint angles, and Null space configuration
q_new = Kinematics.inverse(x_new.vector(), q, null_space)

print('New position: ', x_new)
print('New joints: ', q_new)

文档

可以在https://pantor.github.io/frankx/找到自动生成的文档。此外，示例目录中有多个 C++ 和 Python示例。一旦 frankx 达到 v1.0，我们将添加更详细的文档。

发展

Frankx 是用 C++17 和 Python3.7 编写的。目前针对以下版本进行了测试

• 特征 v3.3.7
• Libfranka v0.9.0
• Pybind11 v2.9.1
• Catch2 v2.13（仅用于测试）

执照

对于非商业应用程序，此软件在 LGPL v3.0 下获得许可。如果您想在商业应用程序中或在不同的许可下使用 frankx，请联系我们以获取单独的协议。