physic.deserializeWorld(world, mechaName)
print "CREATE PORTS..."
phy.addCreateInputPort("clock_trigger", "double")
icps = phy.s.Connectors.IConnectorSynchro.new("icps")
icps.addEvent("clock_trigger")
graph.getPort("body_state_H").connectTo(phy.getPort("body_state_H"))
graph.getPort("contacts").connectTo(phy.getPort("contacts"))
clock.getPort("ticks").connectTo(phy.getPort("clock_trigger"))
print "CREATE Q READER..."
import qreader
qreader = qreader.createQReader("kukaqreader", physic.dynmodel)
qreader.s.start()
rtt_interface_corba.SetServer(qreader._obj)
print "CREATE CONTROLLER..."
#####
# add orcisir_ISIRController
oIT = ddeployer.load("oIT", "Orocos_ISIRController", module="orcisir_Orocos_IsirController-gnulinux", prefix="", libdir=controllerlibdir)
TT = dsimi.rtt.Task(oIT)
phy.getPort(mechaName+"_q").connectTo(TT.getPort("q"))
phy.getPort(mechaName+"_qdot").connectTo(TT.getPort("qDot"))
phy.getPort(mechaName+"_Hroot").connectTo(TT.getPort("Hroot"))
phy.getPort(mechaName+"_Troot").connectTo(TT.getPort("Troot"))
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()
shell()
