from morse.builder import *

# Land robot
morsy = Morsy()
morsy.translate(2.25, -0.75)

pose = Pose()
pose.translate(x=-0.0, z=0.0)
morsy.append(pose)

gripper = Gripper()
gripper.translate(x=0.2000, z=0.5000)
gripper.properties(Angle=360.0, Distance=0.5);
morsy.append(gripper)

camera = VideoCamera()
camera.translate(x=0.2000, z=0.9000)
camera.properties(cam_width=128, cam_height=128);
morsy.append(camera)

waypoint = Waypoint()
waypoint.translate(x=0.0, y=0.0, z=0.0)
waypoint.properties(AngleTolerance=0.1, Speed=50.0)
morsy.append(waypoint)

motion = MotionVW()
morsy.append(motion)

motion.add_service('socket')
gripper.add_service('socket')
pose.add_service('socket')
camera.add_interface('socket')
waypoint.add_interface('socket')



duke = Morsy()
duke.translate(-18.25, -0.25)

pose = Pose()
pose.translate(x=-0.0, z=0.0)
duke.append(pose)

gripper = Gripper()
gripper.translate(x=0.2000, z=0.5000)
gripper.properties(Angle=360.0, Distance=0.5);
duke.append(gripper)

camera = VideoCamera()
camera.translate(x=0.2000, z=0.9000)
camera.properties(cam_width=128, cam_height=128);
duke.append(camera)

waypoint = Waypoint()
waypoint.translate(x=0.0, y=0.0, z=0.0)
waypoint.properties(AngleTolerance=0.1, Speed=50.0)
duke.append(waypoint)

motion = MotionVW()
duke.append(motion)

motion.add_service('socket')
gripper.add_service('socket')
pose.add_service('socket')
camera.add_interface('socket')
waypoint.add_interface('socket')

lara = Morsy()
lara.translate(-8.75, -9.25)

pose = Pose()
pose.translate(x=-0.0, z=0.0)
lara.append(pose)

gripper = Gripper()
gripper.translate(x=0.2000, z=0.5000)
gripper.properties(Angle=360.0, Distance=0.5);
lara.append(gripper)

camera = VideoCamera()
camera.translate(x=0.2000, z=0.9000)
camera.properties(cam_width=128, cam_height=128);
lara.append(camera)

waypoint = Waypoint()
waypoint.translate(x=0.0, y=0.0, z=0.0)
waypoint.properties(AngleTolerance=0.1, Speed=50.0)
lara.append(waypoint)

motion = MotionVW()
lara.append(motion)


motion.add_service('socket')
gripper.add_service('socket')
pose.add_service('socket')
camera.add_interface('socket')
waypoint.add_interface('socket')


env = Environment('/home/orocos/Desktop/cnambot/code/environment_blender/indoor-5')
env.set_camera_rotation([1.0470, 0, 0.7854])

