使用键盘控制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 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 load_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>