#######################################################################################################
print "START ALL..."
sphereWorld = xrl.createWorldFromUrdfFile(xr.sphere, "sphere", [0,0.6,1.2, 1, 0, 0, 0], False, 0.2, 0.005)# , "material.concrete")
kukaWorld = xrl.createWorldFromUrdfFile(xr.kuka, "k1g", [0,0,0.4, 1, 0, 0, 0], True, 1, 0.005) #, "material.concrete")
wm.addMarkers(sphereWorld, ["sphere.sphere"], thin_markers=False)
wm.addWorld(sphereWorld )
wm.addWorld(kukaWorld)
# Create simple gravity compensator controller
import control
controller = control.createTask("controlK1G", TIME_STEP)
controller.connectToRobot(wm.phy, kukaWorld, "k1g")
# Configure the robot
import lgsm
kuka = wm.phy.s.GVM.Robot("k1g")
kuka.enableGravity(True)
kuka.setJointPositions(lgsm.vector([0.4]*7))
kuka.setJointVelocities(lgsm.vector([0.0]*7))
#kuka.enableContactWithBody("sphere.sphere", True)
controller.s.start()
#PDC Control mode
sm = spacemouse.createTask("smi", TIME_STEP, wm.phy, wm.graph, "sphere.sphere", pdc_enabled=True, body_name="k1g.07")
sphereWorld = xrl.createWorldFromUrdfFile(xr.sphere, "sphere", [0,0.6,1.2, 1, 0, 0, 0], False, 0.2, 0.005)# , "material.concrete")
wm.addWorld(groundWorld)
wm.addWorld(kukaWorld)
wm.addWorld(env1World)
wm.addMarkers(sphereWorld, ["sphere.sphere"], thin_markers=False)
wm.addWorld(sphereWorld)
kuka = wm.phy.s.GVM.Robot(mecha_name)
kuka.enableContactWithBody("ground.ground", True)
kuka.enableContactWithBody("env1.env1", True)
# Create simple gravity compensator controller
import control
controller = control.createTask("controlKuka1", TIME_STEP)
controller.connectToRobot(wm.phy, kukaWorld, mecha_name)
#spacemouse
#PDC Control mode
sm = spacemouse.createTask("smi", TIME_STEP, wm.phy, wm.graph, "sphere.sphere", pdc_enabled=True, body_name=mecha_name+".07")
import qreader
qreader = qreader.createQReader("kukaqreader", TIME_STEP)
qreader.connectToRobot(kuka)
rtt_interface_corba.SetServer(qreader._obj)
sm.s.start()
qreader.s.start()
controller.s.start()
wm.startAgents()
