通宵跑通了，下面是改好的两款，一个是get一次然后循环set，第二个是循环get and set：

#!/usr/bin/env python #coding=utf-8 import rospy import geometry_msgs.msg from gazebo_msgs.msg import LinkState from gazebo_msgs.srv import GetLinkState, GetLinkStateRequest, SetLinkState, SetLinkStateRequest from geometry_msgs.msg import Pose, Point, Quaternion, Twist from std_msgs.msg import Header def get_current_state(link_name): """ Get the current pose of the specified link. :param link_name: The name of the link to get the pose for :return: The current pose of the link as a Pose object """ rospy.wait_for_service('/gazebo/get_link_state') try: get_link_state_service = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState) get_link_state_request = GetLinkStateRequest() get_link_state_request.link_name = link_name get_link_state_request.reference_frame = 'world' response = get_link_state_service(get_link_state_request) return response except rospy.ServiceException as e: print("Failed to call get link state service: %s"%e) def set_link_state(link_name, poseinput): """ Set the state of the specified link to keep it static relative to the world. :param link_name: The name of the link to set the state for :param pose: The desired pose, here we set all position and orientation components to 0 """ rospy.wait_for_service('/gazebo/set_link_state') try: set_link_state_service = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) set_link_state_request = SetLinkStateRequest() # Set the link name set_link_state_request.link_state.link_name = link_name set_link_state_request.link_state.reference_frame = 'world' # Set the target pose set_link_state_request.link_state.pose = poseinput # Set the angular and linear velocities to 0 set_link_state_request.link_state.twist = geometry_msgs.msg.Twist(linear= geometry_msgs.msg.Vector3( x = 0, y = 0, z = 0), angular = geometry_msgs.msg.Vector3( x = 0, y = 0, z = 0)) #set_link_state_request.twist.linear.x = 0 #set_link_state_request.twist.linear.y = 0 #set_link_state_request.twist.linear.z = 0 #set_link_state_request.twist.angular.x = 0 #set_link_state_request.twist.angular.y = 0 #set_link_state_request.twist.angular.z = 0 # Send the request and wait for the response result = set_link_state_service(set_link_state_request) return result except rospy.ServiceException as e: print("Failed to call set link state service: %s"%e) if __name__ == '__main__': # Initialize the ROS node rospy.init_node('keep_link_static_example') # Set the link name to keep it static link_name = 'Link_3' # Replace with the name of the link you want to keep static # Get the current pose of the link current_state = get_current_state(link_name) poseoutput = Pose() poseoutput.position.x = current_state.link_state.pose.position.x poseoutput.position.y = current_state.link_state.pose.position.y poseoutput.position.z = current_state.link_state.pose.position.z poseoutput.orientation.w = current_state.link_state.pose.orientation.w poseoutput.orientation.x = current_state.link_state.pose.orientation.x poseoutput.orientation.y = current_state.link_state.pose.orientation.y poseoutput.orientation.z = current_state.link_state.pose.orientation.z rate = rospy.Rate(50) # Set the loop frequency to 50Hz while not rospy.is_shutdown(): if current_state: # Create a Pose message with all position and orientation components set to the current values # Call the function to set the link state success = set_link_state(link_name, poseoutput) # Print the result if success: print("Link '%s' set to static relative to the world successfully." % link_name) else: print("Failed to set link '%s' to static relative to the world." % link_name) else: print("Could not get the current pose of link '%s'." % link_name) rate.sleep() # Sleep based on the set loop frequency

第二个：

#!/usr/bin/env python #coding=utf-8 from gazebo_msgs.srv import GetLinkState, SetLinkState import rospy from gazebo_msgs.msg import LinkState from geometry_msgs.msg import Pose, Point, Quaternion from gazebo_msgs.srv import * # Initialize ROS node rospy.init_node('set_link_state_fixed') # Wait for Gazebo services to start rospy.wait_for_service('/gazebo/get_link_state') rospy.wait_for_service('/gazebo/set_link_state') # Create GetLinkState service proxy get_link_state_proxy = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState) # Create SetLinkState service proxy set_link_state_proxy = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState) # Get the current state of the link link_name = 'Link_3' # Replace with your link name reference_frame= 'world' current_state = get_link_state_proxy(link_name,reference_frame) # Set the new link state to keep the link fixed in its current position link_state = LinkState() link_state.link_name = link_name link_state.pose = current_state.link_state.pose # Keep the current position # Set the linear and angular velocities of the link to zero #link_state.twist = Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0)) link_state.twist.linear.x = 0.0 link_state.twist.linear.y = 0.0 link_state.twist.linear.z = 0.0 link_state.twist.angular.x = 0.0 link_state.twist.angular.y = 0.0 link_state.twist.angular.z = 0.0 link_state.reference_frame = 'world' # Call the SetlinkState service to set the link state set_link_state_proxy(link_state) # Output result print("link state set successfully, link is now fixed in its current position.") # To ensure that the link stays fixed in its current position in future updates, we need to continuously call the SetlinkState service # This can be done using a loop or other logic based on your specific requirements while not rospy.is_shutdown(): set_link_state_proxy(link_state) rospy.sleep(0.002) # Sleep for 0.002 second, adjust frequency as needed