机器人末端力/力矩控制实用简述——以Franka机器人为例

frankapanda
本文以Franka机器人为例简述基本的机器人末端力/力矩控制方法,以及阻抗控制方法。本文假设读者具有一定的机器人学与C++程序设计基础。笔者基于libfranka 0.8.0 版本进行开发调试。除了编程技巧外,本文还将在一定程度上讨论Franka 机器人官方运动生成与阻抗控制方法的基本特征。所有实例代码来自libranka官方手册。更多相关内容建议读者参考此文

笔者开发配置:

  • OS: Ubuntu 18.04 LTS x64
  • RT kernel: 5.6.14-rt6
  • libfranka 0.8.0

机器人末端力控实例

下面代码的作用是使机器人末端在z轴方向产生一个1kg的力,即9.8N,采用PI控制器实现。
(代码来自libranka官方手册实例 force_control.cpp

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <iostream>
#include <Eigen/Core>
#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;
  }
  // parameters
  double desired_mass{
   0.0};
  constexpr double target_mass{
   1.0};    // NOLINT(readability-identifier-naming)
  constexpr double k_p{
   1.0};            // NOLINT(readability-identifier-naming)
  constexpr double k_i{
   2.0};            // NOLINT(readability-identifier-naming)
  constexpr double filter_gain{
   0.001};  // NOLINT(readability-identifier-naming)
  try {
   
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();
    // 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}});
    franka::RobotState initial_state = robot.readOnce();
    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() 
  • 0
    点赞
  • 21
    收藏
    觉得还不错? 一键收藏
  • 5
    评论

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

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值