对于机器人的视觉这些信息，很容易在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
妙啊！！！