#------------------------------------------------------------------------------- import sys import os import inspect cpath = os.path.dirname(os.path.abspath(inspect.getfile( inspect.currentframe()))) + "/" sys.path.append(cpath) cpath = os.path.dirname(os.path.abspath(inspect.getfile( inspect.currentframe()))) + "/../common" sys.path.append(cpath) import common import lgsm phy, ms, lmd = common.get_physic_agent() graph, gInterface = common.get_graphic_agent() #------------------------------------------------------------------------------- # # Create the robot with the desc module. # # The goal is to make a general description of the robot. # The robot is then create in the physical scene with # the deserializeWorld method. # #------------------------------------------------------------------------------- import iCubcommon
# print "-------------------"
# print q
# print qdot
# print q_des_out
# print qdot_des_out
#-------------------------------------------------------------------------------
#
# Setup agents & Create Connections between phy and graph
#
#-------------------------------------------------------------------------------
# create agents
phy, ms, lmd = common.get_physic_agent(time_step) # create physic agent
graph, gInterface = common.get_graphic_agent() # create graphic agent
world = common.create_world_and_deserialized(phy, ms, lmd, graph, gInterface) # create world and deserialize
##### Connect physic and graphic agents to see bodies with markers
ocb = phy.s.Connectors.OConnectorBodyStateList.new("ocb", "bodyPosition")
graph.s.Connectors.IConnectorFrame.new("icf", "framePosition", "mainScene")
graph.getPort("framePosition").connectTo(phy.getPort("bodyPosition_H"))
# add markers on bodies
for n in phy.s.GVM.Scene("main").getBodyNames():
ocb.addBody(n)
gInterface.MarkersInterface.addMarker(n, False)
