sinusoid.py

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

#!/usr/bin/env python

# Desc: Move an object along a sinusoidal path
# Author: Andrew Howard

import math
import time

from gazebo import *


if __name__ == '__main__':

    server = '0' #'default'
    id = 'object1'

    # Connect to server
    client = gz_client()
    if client.connect(0) != 0:
        raise gz_error_str()

    # Open the interface
    truth = gz_truth()
    if truth.open(client, id) != 0:
        raise gz_error_str()

    truth.lock(1)
    start_time = truth.data.time
    start_pos = truth.data.pos[:]
    truth.unlock()

    for i in range(1000):
        time.sleep(0.1)
        
        dt = truth.data.time - start_time
        new_pos = (start_pos[0] + 1.0 * dt,
                   start_pos[1] + 1.0 * math.sin(dt),
                   start_pos[2])
        
        truth.lock(1)
        truth.data.cmd_new = 1
        truth.data.cmd_pos = new_pos
        truth.data.cmd_rot = (1,0,0,0)
        truth.unlock()