通过RViz中的InteractiveMarkers在ROS中仿真力和力矩(wrench.force和wrench.torque)

对于机器人的视觉这些信息，很容易在ROS中呈现，但是对于ROS中力和力矩信息的展示，通常在实验室的时候使用真实的机器人和传感器，这个问题不需要考虑，直接读取真实的力和力矩传感器，然后通过ROS消息发布即可。可当疫情导致的有学不能上，有实验室不能去的时候，想在ROS中检验机器人力控制算法的时候，就可以通过一定的方法在RViz中虚拟出力和力矩信息，通过wrench话题发布。

不仅如此，可以通过RViz的InteractiveMarkers插件展现出来力和力矩的交互属性，原始程序来源于 MIT CSAIL的博士后Nadia Figueroa发布在Github上的ros包https://github.com/nbfigueroa (通过issue还和她聊过^ ^)。我对其进行了简单的修改并将程序单独拿了出来，使其可以用在UR5机械臂的力仿真。

#!/usr/bin/pythonimport rospy
import copy
import tf
import numpyfrom interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
from visualization_msgs.msg import *
from geometry_msgs.msg import Point
from geometry_msgs.msg import Wrench
from tf.broadcaster import TransformBroadcastermarker_pose = geometry_msgs.msg.Transform()
listener = None
wrench_pub = Nonedef publisherCallback( msg ):try:listener.waitForTransform("/world", "/fake_force_pose", rospy.Time(0), rospy.Duration(10.0))(trans1,rot1) = listener.lookupTransform("/world", "/fake_force_pose", rospy.Time(0))(trans2,rot2) = listener.lookupTransform("/world", "/ee_link", rospy.Time(0))(trans3,rot3) = listener.lookupTransform("/ee_link", "/world",rospy.Time(0))# Publish the fake forcefake_wrench = geometry_msgs.msg.WrenchStamped()trans1_mat = tf.transformations.translation_matrix(trans1)trans2_mat = tf.transformations.translation_matrix(trans2)rot3_mat   = tf.transformations.quaternion_matrix(rot3)mat1 = numpy.dot(rot3_mat, trans1_mat-trans2_mat)force_at_ft_link = tf.transformations.translation_from_matrix(mat1)# print(marker_pose)fake_wrench.wrench.force.x = force_at_ft_link[0]fake_wrench.wrench.force.y = force_at_ft_link[1]fake_wrench.wrench.force.z = force_at_ft_link[2]rot = (marker_pose.rotation.x, marker_pose.rotation.y, marker_pose.rotation.z, marker_pose.rotation.w)euler = tf.transformations.euler_from_quaternion(rot)fake_wrench.wrench.torque.x = euler[0]fake_wrench.wrench.torque.y = euler[1]fake_wrench.wrench.torque.z = euler[2]fake_wrench.header.frame_id = "/ee_link"fake_wrench.header.stamp = rospy.Time(0)wrench_pub.publish(fake_wrench)except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):print "Coudn't find transforms!"def transformCallback( msg ):br = TransformBroadcaster()br.sendTransform((marker_pose.translation.x,marker_pose.translation.y,marker_pose.translation.z),(0,0,0,1),rospy.Time.now(),"fake_force_pose","world")def makeBox( msg ):marker = Marker()marker.type = Marker.SPHEREmarker.scale.x = msg.scale * 0.02marker.scale.y = msg.scale * 0.02marker.scale.z = msg.scale * 0.02marker.color.r = 0.0marker.color.g = 1.0marker.color.b = 0.0marker.color.a = 1.0return markerdef makeBoxControl( msg ):control =  InteractiveMarkerControl()control.always_visible = Truecontrol.markers.append( makeBox(msg) )msg.controls.append( control )return controldef processFeedback(feedback, br):s = "Feedback from marker '" + feedback.marker_names += "' / control '" + feedback.control_name + "'"mp = ""if feedback.mouse_point_valid:mp = " at " + str(feedback.mouse_point.x)mp += ", " + str(feedback.mouse_point.y)mp += ", " + str(feedback.mouse_point.z)mp += " in frame " + feedback.header.frame_idif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:# rospy.loginfo(s + ": button click" + mp + ".")passelif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:# rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")passelif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:# rospy.loginfo(s + ": pose changed")marker_pose.translation.x = feedback.pose.position.xmarker_pose.translation.y = feedback.pose.position.ymarker_pose.translation.z = feedback.pose.position.zmarker_pose.rotation.x = feedback.pose.orientation.xmarker_pose.rotation.y = feedback.pose.orientation.ymarker_pose.rotation.z = feedback.pose.orientation.zmarker_pose.rotation.w = feedback.pose.orientation.welif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:pass# rospy.loginfo(s + ": mouse down" + mp + ".")elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:# rospy.loginfo(s + ": mouse up" + mp + ".")passserver.applyChanges()if __name__ == "__main__":rospy.init_node("fake_force_sensor")br = TransformBroadcaster()listener = tf.TransformListener()wrench_pub = rospy.Publisher('/fake_wrench', geometry_msgs.msg.WrenchStamped, queue_size=4)# for default trans and rotation same with postionmarker_pose.translation.x = 0.81725marker_pose.translation.y = 0.19145marker_pose.translation.z = -0.005491marker_pose.rotation.x = 0marker_pose.rotation.y = 0marker_pose.rotation.z = 0marker_pose.rotation.w = 1# Publisher for the topicrospy.Timer(rospy.Duration(0.02), publisherCallback)# Publisher for the TFrospy.Timer(rospy.Duration(0.02), transformCallback)server = InteractiveMarkerServer("fake_force_sensor")menu_handler = MenuHandler()pf_wrap = lambda fb: processFeedback(fb, br)menu_handler.insert("First Entry", callback=pf_wrap)menu_handler.insert("Second Entry", callback=pf_wrap)sub_menu_handle = menu_handler.insert("Submenu")menu_handler.insert("First Entry", parent=sub_menu_handle, callback=pf_wrap)menu_handler.insert("Second Entry", parent=sub_menu_handle, callback=pf_wrap)# for force and torque# position = Point(0, 0, 0)# beacause  admittance_control config file# for force to zeroposition = Point(0.81725, 0.19145, -0.005491)int_marker = InteractiveMarker()int_marker.header.frame_id = "world"int_marker.pose.position = positionint_marker.scale = 0.3int_marker.name = "fake_wrench"int_marker.description = "Sim wrench \n Force = vector from UR5 ee to marker \n Torque = angle from initial orientation"# insert a boxmakeBoxControl(int_marker)fixed = Falsecontrol = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 1control.orientation.y = 0control.orientation.z = 0control.name = "rotate_x"control.interaction_mode = InteractiveMarkerControl.ROTATE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 1control.orientation.y = 0control.orientation.z = 0control.name = "move_x"control.interaction_mode = InteractiveMarkerControl.MOVE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 1control.orientation.z = 0control.name = "rotate_y"control.interaction_mode = InteractiveMarkerControl.ROTATE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 1control.orientation.z = 0control.name = "move_y"control.interaction_mode = InteractiveMarkerControl.MOVE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 0control.orientation.z = 1control.name = "rotate_z"control.interaction_mode = InteractiveMarkerControl.ROTATE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)control = InteractiveMarkerControl()control.orientation.w = 1control.orientation.x = 0control.orientation.y = 0control.orientation.z = 1control.name = "move_z"control.interaction_mode = InteractiveMarkerControl.MOVE_AXISif fixed:control.orientation_mode = InteractiveMarkerControl.FIXEDint_marker.controls.append(control)server.insert(int_marker, pf_wrap)menu_handler.apply(server, int_marker.name)server.applyChanges()rospy.spin()

程序只需要重点关注def publisherCallback( msg )，通过指定markers的中心位置，用ur5末端ee_link到markers的向量来表示力和力矩信息，然后发布在wrench消息上，其中markers的中心位置的初始位置通过主函数中的marker_pose和Point指定，主函数最后的control = InteractiveMarkerControl()赋予markers(力和力矩)交互属性，让我们可以手动调节力和力矩数据大小。

$ roslaunch ur5_moveit_config demo.launch
# 复制代码创建一个ros包
$ rosrun xxx fake_force_torque.py

妙啊！！！

通过RViz中的InteractiveMarkers在ROS中仿真力和力矩(wrench.force和wrench.torque)相关推荐

从零开始学ros小车仿真
从零开始学ros小车仿真从零开始学ros小车仿真目录 1.从零开始学ros小车仿真(一)在solidworks中建模小车并转出为urdf文件 2.从零开始学ros小车仿真(二)在rviz中检验导入 ...
ROS中阶笔记（三）：机器人仿真—ArbotiX+rviz功能仿真
ROS中阶笔记(三):机器人仿真-ArbotiX+rviz功能仿真文章目录 01 机器人URDF模型优化-xacro模型文件 1.1 xacro模型文件 1.2 xacro使用方法 1.3 模型显示 ...
ROS中的TF坐标变换工具及实现、Rviz查看（十四）C++、python
目录 TF坐标变换引言概述概念坐标msg消息简介 geometry_msgs/TransformStamped geometry_msgs/PointStamped 静态坐标变换简介 C++ ...
ros中启动rviz显示段错误，核心以转储问题
ros中启动rviz显示段错误,核心以转储问题运行命令 $ rosrun rviz rviz -d rospack find turtle_tf/rviz/turtle_rviz.rviz 显示解 ...
ROS中base_link, odom, fixed_frame, target_frame和虚拟大地图map的关系
前面已经介绍了如何使用URDF建造机器人小车并显示在Rviz的仿真环境里面,但是小车是静止的.下面介绍如何让它在Rviz里面动起来,并理清URDF,TF 和 odom 的关系. 1. ROS中base ...
linux生成地图,ROS中利用V-rep进行地图构建仿真
V-rep中显示激光扫描点在VREP自带的场景中找到practicalPathPlanningDemo.ttt文件,删除场景中多余的物体只保留静态的地图.然后在Model browser→compo ...
【RK3399Pro学习笔记】十三、ROS中的坐标系管理系统
目录 TF功能包能干什么? TF坐标变换如何实现? 例程 view_frames tf_echo rviz 平台:华硕 Thinker Edge R 瑞芯微 RK3399Pro 固件版本:Tinker ...
一些关于ROS中move_base的理解
move_base是ROS下关于机器人路径规划的中心枢纽.它通过订阅激光雷达.map地图.amcl的定位等数据,然后规划出全局和局部路径,再将路径转化为机器人的速度信息,最终实现机器人导航.这里又要盗 ...
ROS基本概念文件系统创建ROS软件包 ROS中的一些命令
ROS基本概念文件系统创建ROS软件包 ROS中的一些命令 ROS是什么 ROS文件系统文件系统工具:rospack.roscd.rosls 创建ROS 软件包 catkin是什么创建和构建一 ...

通过RViz中的InteractiveMarkers在ROS中仿真力和力矩(wrench.force和wrench.torque)

通过RViz中的InteractiveMarkers在ROS中仿真力和力矩(wrench.force和wrench.torque)相关推荐

最新文章

热门文章