franka cartesian impedance control example

1.代码

franka官方给出的笛卡尔空间下的阻抗控制例程。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
 
#include <Eigen/Dense>
 
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>
 
#include "examples_common.h"
 
int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }
 
  // Compliance parameters
  const double translational_stiffness{150.0};
  const double rotational_stiffness{10.0};
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                     Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
                                         Eigen::MatrixXd::Identity(3, 3);
 
  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();
 
    franka::RobotState initial_state = robot.readOnce();
 
    // equilibrium point is the initial position
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.rotation());
 
    // set collision behavior
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
 
    // define callback for the torque control loop
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables
      std::array<double, 7> coriolis_array = model.coriolis(robot_state);
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
 
      // convert to Eigen
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
      Eigen::Vector3d position(transform.translation());
      Eigen::Quaterniond orientation(transform.rotation());
 
      // compute error to desired equilibrium pose
      // position error
      Eigen::Matrix<double, 6, 1> error;
      error.head(3) << position - position_d;
 
      // orientation error
      // "difference" quaternion
      if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs();
      }
      // "difference" quaternion
      Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
      error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
      // Transform to base frame
      error.tail(3) << -transform.rotation() * error.tail(3);
 
      // compute control
      Eigen::VectorXd tau_task(7), tau_d(7);
 
      // Spring damper system with damping ratio=1
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      tau_d << tau_task + coriolis;
 
      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
      return tau_d_array;
    };
 
    // start real-time control loop
    std::cout << "WARNING: Collision thresholds are set to high values. "
              << "Make sure you have the user stop at hand!" << std::endl
              << "After starting try to push the robot and see how it reacts." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(impedance_control_callback);
 
  } catch (const franka::Exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }
 
  return 0;
}

2.说明

记录一下关键代码的部分。

1
2
3
4
5
6
franka::Robot robot(argv[1]);
setDefaultBehavior(robot);
// load the kinematics and dynamics model
franka::Model model = robot.loadModel();

franka::RobotState initial_state = robot.readOnce();

使用franka::Robot类初始化机械臂，设置默认行为，加载机械臂的运动学和动力学模型，读取机械臂的初始状态。
官方文档给的ip是172.16.0.2。
这里有个坑是，如果用apt安装libfranka和franka_ros库的话，examples_common.h头文件会找不到，需要在libfranka的GitHub仓库手动下载examples_common.h和对应的cpp文件，放到工程目录下。

1
2
3
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
Eigen::Vector3d position_d(initial_transform.translation());
Eigen::Quaterniond orientation_d(initial_transform.rotation());

将机械臂的初始位姿转换为Eigen的Affine3d类型，分别提取出位置和四元数表示的姿态。
这里需要注意initial_state.O_T_EE.data()，libfranka提供的机械臂状态数据是全部是以列优先的array数组。

1
2
std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
    impedance_control_callback = [&](const franka::RobotState& robot_state, franka::Duration /*duration*/) -> franka::Torques {}

前面的function是cpp11的函数容器，后面的lambda表达式是函数体。
简单说明function的用法，<>中间外层是函数的返回值类型，内层是参数类型（括号内）。
lambda表达式的写法是[capture](parameters) -> return_type {body}。
其中capture是捕获列表，parameters是参数列表，return_type是返回值类型，body是函数体。
capture可以是[]，[&]，[=]，[this]，[&a, b]等。
[&]表示引用捕获，[=]表示值捕获，[this]表示捕获当前对象的this指针。
-> return_type是返回值类型，可以省略。
{body}是函数体，可以省略。

1
error.head(3) << position - position_d;

对于位移的误差，直接相减即可。

1
2
3
4
5
6
7
8
9
// "difference" quaternion
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
orientation.coeffs() << -orientation.coeffs();
}
// "difference" quaternion
Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
// Transform to base frame
error.tail(3) << -transform.rotation() * error.tail(3);

首先判断两个四元数的点积是否小于0，如果小于0，说明两个四元数的方向相反，需要取反。

然后通过orientation.inverse() * orientation_d计算姿态误差，这里可以通过旋转矩阵来理解，以\(^{0}{R}_{c}\)(代表当前姿态)和\(^{0}{R}_{d}\)(代表目标姿态)举例，都是相对于基坐标系的表示，那么姿态误差可以表示为\(^{c}{R}_{d} = ^{0}{R}_{c}^{-1} \cdot ^{0}{R}_{d}\)，也就是目标姿态相对于当前姿态的表示，四元数应该也是一样（的吧？）。

最后通过左乘旋转矩阵将姿态误差转换到基坐标系下。

chatgpt给出了另一种做法：

1
2
Eigen::AngleAxisd angle_axis_error(error_quaternion);
error.tail(3) << angle_axis_error.axis() * angle_axis_error.angle();

1
2
tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
tau_d << tau_task + coriolis;

计算力矩，左乘雅可比矩阵的转置将末端力转换为关节力，然后计算阻抗控制力矩，最后加上科里奥利力。
这里的例子没有乘以质量矩阵，大概是为了简化。

3. Links

控制
#franka
franka cartesian impedance control example
https://symcreg.github.io/2024/09/18/franka-cartesian-impedance-control-example/
作者
sam
发布于
2024年9月18日
许可协议