How to change pose of a static model
I have a model (whose <static>
tag is set to 1 in sdf) inserted into the simulation. Can I change its pose continuously through code somehow? I tried running a ROS node like below but it works only if <static>
tag is set to 0.
import rospy
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import SetModelState
from scipy.spatial.transform import Rotation
import numpy as np
from img_getter import *
freq = 2 #hz
deg_x = 1.2981
def main():
rospy.init_node('set_pose')
while not rospy.is_shutdown():
for pos_z in np.linspace(0.7, 1.5, 7):
for deg_x in np.linspace(1.2981, 1.7727, 7):#range (-0.5236, 0.5236, 10):
rot = Rotation.from_euler('xyz', [1.56, deg_x, 1.56], degrees=False)
rot_quat = rot.as_quat()
state_msg = ModelState()
state_msg.model_name = 'camera'
state_msg.pose.position.x = -0.1
state_msg.pose.position.y = 0
state_msg.pose.position.z = pos_z
state_msg.pose.orientation.x = rot_quat[0]
state_msg.pose.orientation.y = rot_quat[1]
state_msg.pose.orientation.z = rot_quat[2]
state_msg.pose.orientation.w = rot_quat[3]
rate = rospy.Rate(freq)
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
resp = set_state( state_msg )
print('changed state')
rate.sleep()
except rospy.ServiceException as e:
print("Service call failed: {}".format(e))
break
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
pass