6 篇文章 3 订阅

• C++基础
• Eigen基础
• 机器人学基础

# libfranka综述

libfranka是采用C++开发的工具包，用以开发Franka机器人的应用。libfranka提供了实时的控制机制，使能用户开发复杂的运动规划/控制算法，十分方便。Franka机器人的控制频率为 1kHz ，但是实际留给用户代码的执行时间 <300 μ \mu s。切记不要让代码过于冗长，也不要太多额外的外部读写操作。

# 运动生成

## 关节空间运动生成

When you are using a joint motion generator, the Robot kinematics completion block will not modify the commanded joint values and therefore q d , q ˙ d , q ¨ d q_d, \dot{q}_d, \ddot{q}_d and q c , q ˙ c , q ¨ c q_c, \dot{q}_c, \ddot{q}_c are equivalent. Note that you will only find the d d signals in the robot state.

std::array<double, 7> initial_position;
double time = 0.0;
robot.control([&initial_position, &time](const franka::RobotState& robot_state,
franka::Duration period) -> franka::JointPositions {
time += period.toSec();
if (time == 0.0) {
initial_position = robot_state.q_d;
}
double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
franka::JointPositions output = {{initial_position[0], initial_position[1],
initial_position[2], initial_position[3] + delta_angle,
initial_position[4] + delta_angle, initial_position[5],
initial_position[6] + delta_angle}};
if (time >= 5.0) {
std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
return franka::MotionFinished(output);
}
return output;
});


δ θ = π 8 ( 1 − cos ⁡ ( 2 π 5 t ) ) \delta\theta = \frac{\pi}{8} \left(1-\cos\left(\frac{2\pi}{5}t\right)\right) 可以看出，这是一个S形曲线运动规划。使用正余弦函数可以保证轨迹在任何位置无穷阶可微，即保证了光滑性。此时只要限制振幅（此处为 π / 8 \pi/8 ）就可以确保速度、加速度、加加速度不超限。

double delta_angle = M_PI / 64.0;
franka::JointPositions output = {{initial_position[0], initial_position[1],
initial_position[2], initial_position[3],
initial_position[4], initial_position[5],
initial_position[6] + delta_angle}};


## 笛卡尔空间运动生成

    // First move the robot to a suitable joint configuration
std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
MotionGenerator motion_generator(0.5, q_goal);
std::cout << "WARNING: This example will move the robot! "
<< "Please make sure to have the user stop button at hand!" << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
robot.control(motion_generator);
std::cout << "Finished moving to initial joint configuration." << std::endl;
// Set additional parameters always before the control loop, NEVER in the control loop!
// Set collision behavior.
robot.setCollisionBehavior(
{{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
{{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
{{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
{{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
std::array<double, 16> initial_pose;
double time = 0.0;
robot.control([&time, &initial_pose](const franka::RobotState& robot_state,
franka::Duration period) -> franka::CartesianPose {
time += period.toSec();
if (time == 0.0) {
initial_pose = robot_state.O_T_EE_c;
}
double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time));
double delta_x = kRadius * std::sin(angle);
double delta_z = kRadius * (std::cos(angle) - 1);
std::array<double, 16> new_pose = initial_pose;
new_pose[12] += delta_x;
new_pose[14] += delta_z;
if (time >= 10.0) {
std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
return franka::MotionFinished(new_pose);
}
return new_pose;
});


# 阻抗控制与力控

Franka机器人的一大优势就是直接实时控制关节力矩，这让用户可以自由设计复杂控制策略。

## 以力矩为输入的运动控制

  // Set and initialize trajectory parameters.
const double vel_max = 0.25;
const double acceleration_time = 2.0;
const double run_time = 20.0;

double vel_current = 0.0;
double angle = 0.0;
double time = 0.0;
// Define callback function to send Cartesian pose goals to get inverse kinematics solved.
auto cartesian_pose_callback = [=, &time, &vel_current, &running, &angle, &initial_pose](
const franka::RobotState& robot_state,
franka::Duration period) -> franka::CartesianPose {
time += period.toSec();
if (time == 0.0) {
// Read the initial pose to start the motion from in the first time step.
initial_pose = robot_state.O_T_EE_c;
}
// Compute Cartesian velocity.
if (vel_current < vel_max && time < run_time) {
vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
}
if (vel_current > 0.0 && time > run_time) {
vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
}
vel_current = std::fmax(vel_current, 0.0);
vel_current = std::fmin(vel_current, vel_max);
// Compute new angle for our circular trajectory.
angle += period.toSec() * vel_current / std::fabs(radius);
if (angle > 2 * M_PI) {
angle -= 2 * M_PI;
}
// Compute relative y and z positions of desired pose.
double delta_y = radius * (1 - std::cos(angle));
double delta_z = radius * std::sin(angle);
franka::CartesianPose pose_desired = initial_pose;
pose_desired.O_T_EE[13] += delta_y;
pose_desired.O_T_EE[14] += delta_z;
// Send desired pose.
if (time >= run_time + acceleration_time) {
running = false;
return franka::MotionFinished(pose_desired);
}
return pose_desired;
};
// Set gains for the joint impedance control.
// Stiffness
const std::array<double, 7> k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}};
// Damping
const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}};
// Define callback for the joint torque control loop.
std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
impedance_control_callback =
[&print_data, &model, k_gains, d_gains](
const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques {
// Read current coriolis terms from model.
std::array<double, 7> coriolis = model.coriolis(state);
// Compute torque command from joint impedance control law.
// Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one
// time step delay.
std::array<double, 7> tau_d_calculated;
for (size_t i = 0; i < 7; i++) {
tau_d_calculated[i] =
k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
}
// Send torque command.
return tau_d_rate_limited;
};
// Start real-time control loop.
robot.control(impedance_control_callback, cartesian_pose_callback);


• 这个程序完成了什么功能？——控制机器人末端在y-z平面执行圆周运动。
• 这个程序与前述运动生成有何区别？——采用了专门定义的控制器，并非默认阻抗控制器。
• 理论上是如何实现的？——梯形速度规划，原理比较简单，此处不做详述。
• 值得注意的是：控制信号是关节力矩，控制目标是机器人末端笛卡尔空间的运动。那么这就绕不开机器人的逆向运动学问题（逆解）。然而Franka机器人是个7自由度冗余的机构，逆解问题是 ill-posed problem 。好在 Franka 机器人提供自带的逆解，即 q d q_{d} ，注意有一个控制周期的延迟，也就是上一时刻的逆解。这个逆解虽然能用，但是插值做不好也容易出问题，建议慎用之。

  const double radius = 0.05;
const double vel_max = 0.25;
const double acceleration_time = 2.0;
const double run_time = 20.0;


      time += period.toSec();
if (time == 0.0) {
// Read the initial pose to start the motion from in the first time step.
initial_pose = robot_state.O_T_EE_c;
}


if (vel_current < vel_max && time < run_time) {
vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
}
if (vel_current > 0.0 && time > run_time) {
vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
}
vel_current = std::fmax(vel_current, 0.0);
vel_current = std::fmin(vel_current, vel_max);


      // Compute new angle for our circular trajectory.
angle += period.toSec() * vel_current / std::fabs(radius);
if (angle > 2 * M_PI) {
angle -= 2 * M_PI;
}


      // Compute relative y and z positions of desired pose.
double delta_y = radius * (1 - std::cos(angle));
double delta_z = radius * std::sin(angle);
franka::CartesianPose pose_desired = initial_pose;
pose_desired.O_T_EE[13] += delta_y;
pose_desired.O_T_EE[14] += delta_z;


// Set gains for the joint impedance control.
// Stiffness
const std::array<double, 7> k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}};
// Damping
const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}};


      // Read current coriolis terms from model.
std::array<double, 7> coriolis = model.coriolis(state);
// Compute torque command from joint impedance control law.
// Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one
// time step delay.
std::array<double, 7> tau_d_calculated;
for (size_t i = 0; i < 7; i++) {
tau_d_calculated[i] =
k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
}
// Send torque command.
return tau_d_rate_limited;


## 外力估计与控制

    Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
// Bias torque sensor
std::array<double, 7> gravity_array = model.gravity(initial_state);
Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
initial_tau_ext = initial_tau_measured - initial_gravity;
// init integrator
tau_error_integral.setZero();
// define callback for the torque control loop
Eigen::Vector3d initial_position;
double time = 0.0;
auto get_position = [](const franka::RobotState& robot_state) {
return Eigen::Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13],
robot_state.O_T_EE[14]);
};
auto force_control_callback = [&](const franka::RobotState& robot_state,
franka::Duration period) -> franka::Torques {
time += period.toSec();
if (time == 0.0) {
initial_position = get_position(robot_state);
}
if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
throw std::runtime_error("Aborting; too far away from starting pose!");
}
// get state variables
std::array<double, 42> jacobian_array =
model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
desired_force_torque.setZero();
desired_force_torque(2) = desired_mass * -9.81;
tau_ext << tau_measured - gravity - initial_tau_ext;
tau_d << jacobian.transpose() * desired_force_torque;
tau_error_integral += period.toSec() * (tau_d - tau_ext);
// FF + PI control
tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;
// Smoothly update the mass to reach the desired target value
desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;
std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
return tau_d_array;
};


• 首先，这个程序用以实现什么功能?——假设机器人末端与一个刚性平面垂直接触，控制机器人末端z轴方向产生一个1kg的力（ 9.81 × 1 ( N ) 9.81\times 1(N) ）。
• 理论上是如何实现的？——以关节力矩作为输入的PI控制器： τ c = τ d + K P ( τ d − τ e x t ) + K I ∫ 0 t τ e ( s ) d s \tau_{c} = \tau_{d} + K_{P}(\tau_{d} - \tau_{ext}) + K_{I}\int_{0}^{t}\tau_{e}(s)\mathrm{d}s 其中 τ c \tau_{c} 是力矩指令， τ d \tau_{d} 是期望力矩， τ e x t \tau_{ext} 是外部力矩， τ e \tau_{e} 是偏差。 τ e = τ d − τ e x t \tau_{e} =\tau_{d} -\tau_{ext}
• 这里多说一句 τ e x t \tau_{ext} ：这个值并非测量值 τ m \tau_{m} ，而是 τ e x t ( t ) = τ m ( t ) − g ( q ) − τ e x t ( 0 ) \tau_{ext}(t) = \tau_{m}(t) - g(q) -\tau_{ext}(0) 。也就是代码注释中所提及的 bias torque sensor。也就是说，通过RobotStarte结构体读取的力矩（tau_J）本身并不包含重力补偿，是单纯的传感器测量值。同时，使用时还要注意初始状态下必然存在偏置力矩（ τ e x t ( 0 ) \tau_{ext}(0) ）。
• 本实例是在笛卡尔空间控制力，然而控制信号是关节力矩，因此需要通过雅可比矩阵推算期望力矩 τ d \tau_{d} 。好在libfranka的模型库 franka::Model 提供了雅可比矩阵的计算方法，可以直接调用： τ d = J T ( q ) f d \tau_{d} = J^{T}(q)f_{d}

// Bias torque sensor
std::array<double, 7> gravity_array = model.gravity(initial_state);
Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
initial_tau_ext = initial_tau_measured - initial_gravity;


if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
throw std::runtime_error("Aborting; too far away from starting pose!");
}


 // get state variables
std::array<double, 42> jacobian_array =
model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());


desired_force_torque.setZero();
desired_force_torque(2) = desired_mass * -9.81;
tau_ext << tau_measured - gravity - initial_tau_ext;
tau_d << jacobian.transpose() * desired_force_torque;


tau_error_integral += period.toSec() * (tau_d - tau_ext);
// FF + PI control
tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;
// Smoothly update the mass to reach the desired target value
desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;


## 阻抗控制

  // 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);
// connect to robot
franka::Robot robot(argv[1]);
setDefaultBehavior(robot);
// load the kinematics and dynamics model
// 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.linear());
// 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.linear());
// compute error to desired equilibrium pose
// position error
Eigen::Matrix<double, 6, 1> error;
// 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.linear() * error.tail(3);
// compute control
// Spring damper system with damping ratio=1
tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));

std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
return tau_d_array;
};


• 这个程序完成了什么功能?——固定末端位姿的阻抗控制，即机器人末端模拟一个弹簧阻尼机构。
• 理论上是如何实现的？ τ d = J T ( q ) ( − α ⋅ e − β ⋅ J ( q ) q ˙ ) + C ( q , q ˙ ) \tau_{d} = J^{T}(q)\left( -\alpha\cdot e - \beta\cdot J(q)\dot{q} \right) + C(q,\dot{q}) 其中 α \alpha β \beta 分别为刚度（弹簧）和阻尼， e e 是位姿偏差。科氏力 C ( q , q ˙ ) C(q,\dot{q}) 比较小，影响不大。注意机器人本身自带重力补偿。
• 本例中雅可比矩阵和科氏力均是通过 libfranka 提供的 franka::Model 类获取，注意构造函数输入可以是 franka::RobotState 结构体。
• 这里多说一点偏差 e e ，这个偏差时位姿与姿态的混合偏差 （ e ∈ R 6 e\in\mathbb{R}^{6} ），此处位置偏差用欧式距离，姿态偏差用四元数虚部之差。这个做法在工程上可用，但是并非十分完善，该问题此处暂不做讨论。

  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);


    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.linear());


      // 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.linear());


      // position error
Eigen::Matrix<double, 6, 1> error;
// orientation error
// "difference" quaternion
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
orientation.coeffs() << -orientation.coeffs();
}
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.linear() * error.tail(3);


      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));


# 其它参考

• 8
点赞
• 51
收藏
觉得还不错? 一键收藏
• 打赏
• 22
评论
07-28
07-28
03-31 47
07-28
05-25 5928
03-11 268
09-07 548
07-28
07-28

### “相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

¥2 ¥4 ¥6 ¥10 ¥20

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、C币套餐、付费专栏及课程。