使用键盘控制Franka机械臂运动

功能说明

使用键盘按键，可以控制franka机械臂7个关节角，已在真机上验证。

代码

主要使用的是官方包内的 franka_example_controllers

1、修改 include下的 joint_position_example_controller.h, 改为如下：

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
//  技术交流/服务 v： L2622452304
#pragma once

#include <array>
#include <string>
#include <vector>

#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>

namespace franka_example_controllers {

class JointPositionExampleController : public controller_interface::MultiInterfaceController<
                                           hardware_interface::PositionJointInterface> {
 public:
  ros::NodeHandle n;
  bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
  void joint_command_callback(const std_msgs::Float32MultiArray &msg);
  void starting(const ros::Time&) override;
  void update(const ros::Time&, const ros::Duration& period) override;
  std::array<float, 7> joints_position_change = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
  
 private:
  hardware_interface::PositionJointInterface* position_joint_interface_;
  std::vector<hardware_interface::JointHandle> position_joint_handles_;
  ros::Duration elapsed_time_;
  std::array<double, 7> initial_pose_{};

};

}  // namespace franka_example_controllers

2、修改 src下的 joint_position_example_controller.cpp, 改为如下：

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka_example_controllers/joint_position_example_controller.h>

#include <cmath>

#include <controller_interface/controller_base.h>
#include <hardware_interface/hardware_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
// #include <ros/ros.h>


namespace franka_example_controllers {

bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware,
                                          ros::NodeHandle& node_handle) {
  n = node_handle;
  position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>();
  if (position_joint_interface_ == nullptr) {
    ROS_ERROR(
        "JointPositionExampleController: Error getting position joint interface from hardware!");
    return false;
  }
  std::vector<std::string> joint_names;
  if (!node_handle.getParam("joint_names", joint_names)) {
    ROS_ERROR("JointPositionExampleController: Could not parse joint names");
  }
  if (joint_names.size() != 7) {
    ROS_ERROR_STREAM("JointPositionExampleController: Wrong number of joint names, got "
                     << joint_names.size() << " instead of 7 names!");
    return false;
  }
  position_joint_handles_.resize(7);
  for (size_t i = 0; i < 7; ++i) {
    try {
      position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]);
    } catch (const hardware_interface::HardwareInterfaceException& e) {
      ROS_ERROR_STREAM(
          "JointPositionExampleController: Exception getting joint handles: " << e.what());
      return false;
    }
  }

  std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
  for (size_t i = 0; i < q_start.size(); i++) {
    if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) {
      ROS_ERROR_STREAM(
          "JointPositionExampleController: Robot is not in the expected starting position for "
          "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
          "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
      return false;
    }
  }

  return true;
}

void JointPositionExampleController::joint_command_callback(const std_msgs::Float32MultiArray &msg){
  for (size_t i = 0; i < 7; ++i) {
    joints_position_change[i] = msg.data[0];
  }
}

void JointPositionExampleController::starting(const ros::Time& /* time */) {
  // for (size_t i = 0; i < 7; ++i) {
  //   initial_pose_[i] = position_joint_handles_[i].getPosition();
  // }
  // elapsed_time_ = ros::Duration(0.0);

  // ros::init("keyboard_subscriber");
  ros::Subscriber joint_sub = n.subscribe("/keyboard", 1, &JointPositionExampleController::joint_command_callback,this);
  ros::spin();
}

void JointPositionExampleController::update(const ros::Time& /*time*/,
                                            const ros::Duration& period) {
  // elapsed_time_ += period;

  // double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec())) * 0.2;
  // for (size_t i = 0; i < 7; ++i) {
  //   if (i == 4) {
  //     position_joint_handles_[i].setCommand(initial_pose_[i] - delta_angle);
  //   } else {
  //     position_joint_handles_[i].setCommand(initial_pose_[i] + delta_angle);
  //   }
  // }

  for (size_t i = 0; i < 7; ++i) {
    initial_pose_[i] = position_joint_handles_[i].getPosition();
  }
  for (size_t i = 0; i < 7; ++i) {
    position_joint_handles_[i].setCommand(initial_pose_[i] + joints_position_change[i]);
    joints_position_change[i] = 0.0;
  }
}

}  // namespace franka_example_controllers

PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointPositionExampleController,
                       controller_interface::ControllerBase)

3、在package下新建scripts文件夹，新建 joint_position_keyboard.py 程序，如下：

#! /usr/bin/env python
#  技术交流 /服务v： L2622452304
import rospy
from std_msgs.msg import Float32MultiArray
import sys, select, termios, tty
from threading import Thread

joints_change = {
        '1':(0,0.01),
        'q':(0,-0.01),
        '2':(1,0.01),
        'w':(1,-0.01),
        '3':(2,0.01),
        'e':(2,-0.01),
        '4':(3, 0.01),
        'r':(3,-0.01),
        '5':(4, 0.01),
        't':(4,-0.01),
        '6':(5, 0.01),
        'y':(5,-0.01),
        '7':(6, 0.01),
        'u':(6,-0.01),
           }
key = ''
rospy.init_node('keyboard_pub')
pub = rospy.Publisher('/keyboard', Float32MultiArray, queue_size=1)

def getKey():
    global key
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
        if key == 'z':
            print('Stop')
            exit(0)
    else:
        key = ''
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

def pub_command():
    global key
    global pub
    keyboard_command = Float32MultiArray()
    rate = rospy.Rate(100)
    while True:
        joints = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        if key in joints_change.keys():
            joints[joints_change[key][0]] += joints_change[key][1]
            # print(joints)
        
        keyboard_command.data = joints
        pub.publish(keyboard_command)
        rate.sleep()


if __name__ == '__main__':
    
    settings = termios.tcgetattr(sys.stdin)
    t = Thread(target=pub_command)
    t.start()
    try:
        while True:
            getKey()
            
    except rospy.ROSInterruptException:
        pass
    
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

实现原理

利用官方给的示例代码，稍作改动来驱动机械臂，然后用topic通信，把py获取的键盘控制信息发送到cpp内的机械臂控制循环中，实现关节运动控制。

使用

启动launch下的 joint_position_example_controller.launch,

<?xml version="1.0" ?>
<launch>
  <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
  <arg name="arm_id" default="panda"/>
  <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" />
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="joint_position_example_controller"/>
  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz -f $(arg arm_id)_link0 --splash-screen $(find franka_visualization)/splash.png"/>
</launch>

也可实现键盘控制机械臂末端移动，但是代码为找到。后续可能继续更新在仿真中的实现。