使用键盘控制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_controllersPLUGINLIB_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 Threadjoints_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 keytty.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 keyglobal pubkeyboard_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 = jointspub.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:passtermios.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>

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