from openravepy import *
from numpy import *
env=Environment()
env.SetViewer('qtcoin')
env.Load('data/lab1.env.xml')
robot=env.GetRobots()[0]
while True:
    T=robot.GetTransform()
    dir=array([1,0,0])  # foward direction in robot frame
    globaldir=dot(T[0:3,0:3],dir) # global direction
    T[0:3,3]+= globaldir*0.001 # move forward a little
    robot.SetTransform(T) # set new transform
    if env.CheckCollision(robot): # check collision
        break