How to change pose of a static model

asked 2022-03-02 10:08:13 -0600

user1928 gravatar image

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
edit retag flag offensive close merge delete