def __init__(self, name, time_step, phy, graph, cursor_name, pdc_enabled, body_name, PR, PT, DR, DT):
"""
"""
super(SpaceMouse, self).__init__(rtt_interface.PyTaskFactory.CreateTask(name))
self.s.setPeriod(time_step)
self.smf = xdefw.rtt.Task(ddeployer.load("smf", "dio::SpaceMouseFactory", "dio-hw-spacemouse", "dio/factory/"))
self.smf.s.setPeriod(time_step)
self.device = self.smf.s.scan()
self.sm = xdefw.rtt.Task(self.smf.s.build(self.device[0], 'smf'))
self.sm.s.setPeriod(time_step)
self.phy = phy
self.camera_interface = graph.s.Interface.CameraInterface("mainScene")
self.sm_in_port = self.addCreateInputPort("sm_in", "Twistd" )
self.sm_in_port.connectTo(self.sm.getPort("out_vel"))
self.pos_out = self.addCreateOutputPort("pos_out", "Displacementd")
self.cursor = None
self.cursor = self.phy.s.GVM.RigidBody(cursor_name)
self.cursor.disableWeight()
self.pdc_enabled = pdc_enabled
self.PR = PR
self.PT = PT
self.DR = DR
self.DT = DT
if self.pdc_enabled == True:
if body_name is not None:
self.createPDC(body_name)
self.camera = lgsm.Displacement()
def createClockAgent(self, time_step, clock_name="clock"):
""" Create clock agent.
:param double time_step: the tick period (in s)
:param string clock_name: the name of the clock agent
It initializes the following attribute in this world manager:
* clock
"""
verbose_print("CREATE CLOCK...")
self.clock = xdefw.rtt.Task(ddeployer.load(clock_name, "dio::Clock", "dio-cpn-clock", ""))
self.clock.s.setPeriod(time_step)
def __init__(self, name, time_step, phy, graph, cursor_name, pdc_enabled,
body_name, PR, PT, DR, DT):
"""
"""
super(SpaceMouse,
self).__init__(rtt_interface.PyTaskFactory.CreateTask(name))
self.s.setPeriod(time_step)
self.smf = xdefw.rtt.Task(
ddeployer.load("smf", "dio::SpaceMouseFactory",
"dio-hw-spacemouse", "dio/factory/"))
self.smf.s.setPeriod(time_step)
self.device = self.smf.s.scan()
self.sm = xdefw.rtt.Task(self.smf.s.build(self.device[0], 'smf'))
self.sm.s.setPeriod(time_step)
self.phy = phy
self.camera_interface = graph.s.Interface.CameraInterface("mainScene")
self.sm_in_port = self.addCreateInputPort("sm_in", "Twistd")
self.sm_in_port.connectTo(self.sm.getPort("out_vel"))
self.pos_out = self.addCreateOutputPort("pos_out", "Displacementd")
self.cursor = None
self.cursor = self.phy.s.GVM.RigidBody(cursor_name)
self.cursor.disableWeight()
self.pdc_enabled = pdc_enabled
self.PR = PR
self.PT = PT
self.DR = DR
self.DT = DT
self.count = 1
self.v_y = 0.05
self.X_x = 0.9
self.mov_dir = "aller"
if self.pdc_enabled == True:
if body_name is not None:
self.createPDC(body_name)
self.camera = lgsm.Displacement()
def __init__(self, name, time_step, phy, graph, cursor_name, pdc_enabled, body_name):
"""
"""
super(Obj_Mouse, self).__init__(rtt_interface.PyTaskFactory.CreateTask(name))
self.s.setPeriod(time_step)
self.smf = xdefw.rtt.Task(ddeployer.load("smf", "dio::obj_Movefactory", "dio-hw-obj_move", "dio/factory/"))
self.smf.s.setPeriod(time_step)
self.device = self.smf.s.scan()
self.sm = xdefw.rtt.Task(self.smf.s.build(self.device[0], 'smf'))
self.sm.s.setPeriod(time_step)
self.phy = phy
self.camera_interface = graph.s.Interface.CameraInterface("mainScene")
self.sm_in_port = self.addCreateInputPort("sm_in", "Twistd" )
self.sm_in_port.connectTo(self.sm.getPort("out_vel"))
self.pos_out = self.addCreateOutputPort("pos_out", "Displacementd")
self.cursor = None
self.cursor = self.phy.s.GVM.RigidBody(cursor_name)
self.cursor.disableWeight()
print "BEGIN OF SCRIPT..."
print "CREATE SCENE..."
import scene1
mechaName = "kuka1"
world = scene1.buildKuka(mechaName)
worldKuka1 = scene1.buildKuka(mechaName)
scene1.addContactLaws(world)
scene1.addGround(world)
scene1.addWall(world)
scene1.addCollisionPairs(world, "ground", mechaName)
scene1.addCollisionPairs(world, "env1", mechaName)
clock = dsimi.rtt.Task(ddeployer.load("clock", "dio::Clock", "dio-cpn-clock", "dio/component/"))
clock.s.setPeriod(TIME_STEP)
print "CREATE GRAPHIC..."
import graphic
graph = graphic.createTask()
graphic.init()
graphic.deserializeWorld(world)
print "CREATE PHYSIC..."
import physic
phy = physic.createTask()
physic.init(TIME_STEP)
physic.createDynamicModel(worldKuka1, mechaName)
deploy.loadTypekitsAndPlugins()
import time
#-------------------------------------------------------------------------------
#
# Create graphic agent with orocos rtt.Task
#
#-------------------------------------------------------------------------------
##### Create graphic agent: orocos task
import xdefw.rtt
import deploy.deployer as ddeployer
graph = xdefw.rtt.Task(ddeployer.load("graphic", "xdefw::graphics::GraphicAgent", "xdefw-agt-graphics"),
binding_class=xdefw.rtt.ObjectStringBinding,
static_classes=['Viewer', 'Dio', 'agent', 'Connectors'])
##### Configure graphic agent
# Configure Ogre
ogre_resource_path = loader.share_dir+'/resources/ogre'
ogre_other_resources = [ # get the Ogre3D resources path (material definitions, etc.)
loader.share_dir+'/resources/ogre/meshes',
loader.share_dir+'/resources/ogre/materials/textures',
loader.share_dir+'/resources/ogre/materials/textures/ocean',
loader.share_dir+'/resources/ogre/materials/programs',
loader.share_dir+'/resources/ogre/materials/core'
]
# -------------------------------------------------------------------------------
#
# Create Controller
#
# -------------------------------------------------------------------------------
# Create controller
controller = MyController("MyController", world, "p1") # ControllerName, the world instance, RobotName
controller.s.setPeriod(0.001)
##### Create clock, to synchronize phy and controller
import deploy.deployer as ddeployer
clock = xdefw.rtt.Task(ddeployer.load("clock", "dio::Clock", "dio-cpn-clock", ""))
clock.s.setPeriod(0.01)
# add Input Port in physic agent to be able to send Joint torques to a robot
phy.s.Connectors.IConnectorRobotJointTorque.new("ict", "p1_", "p1") # ConnectorName, PortName, RobotName
# It generates a port named "PortName"+"tau"
# Create a port in the physic agent to receive ticks
phy.addCreateInputPort("clock_trigger", "double")
# Create a Synchronization connectors: when all events of the connector is received,
# the updateHook, and by extension the computation of the physic, of the agent is triggered.
icps = phy.s.Connectors.IConnectorSynchro.new("icps")
# Add event to the connector involving the reception of a new torque command.
icps.addEvent("p1_tau")
# Add event to the connector involving the reception of a new tick.
icps.addEvent("clock_trigger")
TIME_STEP = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(TIME_STEP)
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)
#Controller
control = xdefw.rtt.Task(ddeployer.load("control", "XDE_SimpleController", "XDESimpleController-gnulinux", "", libdir="/tmp/XDE-SimpleController/_build/src/"))
import xde.desc.physic.physic_pb2
model = xde.desc.physic.physic_pb2.MultiBodyModel()
model.kinematic_tree.CopyFrom(kukaWorld.scene.physical_scene.nodes[0])
model.meshes.extend(kukaWorld.library.meshes)
model.mechanism.CopyFrom(kukaWorld.scene.physical_scene.mechanisms[0])
model.composites.extend(kukaWorld.scene.physical_scene.collision_scene.meshes)
dynmodel = physicshelper.createDynamicModel(model)
control.s.setDynModel(str(dynmodel.this.__long__()))
#create connectors to get robot k1g state 'k1g_q', 'k1g_qdot', 'k1g_Hroot', 'k1g_Troot', 'k1g_H'
robot_name = "k1g"
wm.phy.s.Connectors.OConnectorRobotState.new("ocpos"+robot_name, robot_name+"_", robot_name)
wm.phy.s.Connectors.IConnectorRobotJointTorque.new("ict"+robot_name, robot_name+"_", robot_name)
import xde_spacemouse as spacemouse
##### GROUND
ground_fixed_base = True
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,-0.8773,1,0,0,0], ground_fixed_base, 0.001, 0.001)
wm.addWorld(groundWorld)
##### ROBOT
rname = "romeo"
robot_fixed_base = False
romeoWorld = xrl.createWorldFromUrdfFile(xr.romeo_collision, rname, [0,0,0.0,1,0,0,0], robot_fixed_base, 0.003, 0.001)
wm.addWorld(romeoWorld)
#Controller
control = xdefw.rtt.Task(ddeployer.load("control", "ISIRControllerThreadXDE", "ISIRControllerThreadXDE-gnulinux", "", libdir="../../_build/"))
#import xde.desc.physic.physic_pb2
#model = xde.desc.physic.physic_pb2.MultiBodyModel()
#model.kinematic_tree.CopyFrom(romeoWorld.scene.physical_scene.nodes[0])
#model.meshes.extend(romeoWorld.library.meshes)
#model.mechanism.CopyFrom(romeoWorld.scene.physical_scene.mechanisms[0])
#model.composites.extend(romeoWorld.scene.physical_scene.collision_scene.meshes)
#dynmodel = physicshelper.createDynamicModel(model)
dynmodel = xrl.getDynamicModelFromWorld(romeoWorld)
romeo = wm.phy.s.GVM.Robot(rname)
romeo.enableGravity(True)
jointmap = xrl.getJointMapping(xr.romeo_collision, romeo)
N = romeo.getJointSpaceDim()
def get_clock_agent(time_step=0.01):
##### Create clock
clock = xdefw.rtt.Task(ddeployer.load("clock", "dio::Clock", "dio-cpn-clock", ""))
clock.s.setPeriod(time_step)
return clock
kukacommon.addSphere(world)
kukacommon.addObst_1(world)
kukacommon.addObst_2(world)
kukacommon.addContactLaws(world)
kukacommon.addCollision_with_ground(world)
kukacommon.addCollision_with_sphere(world)
kukacommon.addCollision_with_obst_1(world)
kukacommon.addCollision_with_obst_2(world)
wm.addWorld(world)
#wm.graph_scn.SceneInterface.setNodeMaterial('ground', 'xde/ConcretGround', True) # Pour changer la couleur du mannequin
############################### Controller & Dynamic model ###############################
# dsimi.rtt : convert RTT::TaskContext * into a python object /
control = xdefw.rtt.Task(ddeployer.load("control", "XDE_SimpleController", "XDESimpleController-gnulinux", "", libdir="/home/anis/ros_workspace/kuka_controller/lib/orocos/gnulinux"))
kukaModel = xde.desc.physic.physic_pb2.MultiBodyModel()
kukaModel.kinematic_tree.CopyFrom(world.scene.physical_scene.nodes[0])
kukaModel.meshes.extend(world.library.meshes)
kukaModel.mechanism.CopyFrom(world.scene.physical_scene.mechanisms[0])
kukaModel.composites.extend(world.scene.physical_scene.collision_scene.meshes)
model = physicshelper.createDynamicModel(kukaModel)
#model.setJointPositions(lgsm.vectord(-0.5, -0.3, -0.4, -0.4, -0.2, -0.2, -0.2))
#model.setJointPositions(lgsm.vectord(0.05, 0.68, 0.68, 0.68, 0.68, 0.68, 0.68))
#model.setJointPositions(lgsm.vectord(3.5, 0.68, -0.5, 0.68, 0.68, 0.68, 0.68))
model.setJointPositions(lgsm.vectord(0.5, 0.68, 0.0, 1.5, 0, 0.0, 0.0)) #Position articulaire de depart du robot
#model.setJointPositions(lgsm.vectord(0, 3.14159265359/2, 0, 0, 0, 0.0, 0.0))
#model.setJointPositions(lgsm.vectord(0, 0, 0, 0, 0, 0.0, 0.0))
def get_clock_agent(time_step=0.01):
##### Create clock
clock = xdefw.rtt.Task(
ddeployer.load("clock", "dio::Clock", "dio-cpn-clock", ""))
clock.s.setPeriod(time_step)
return clock
import loader
import deploy
deploy.loadTypekitsAndPlugins()
#-------------------------------------------------------------------------------
#
# Create physic agent with orocos rtt.Task
#
#-------------------------------------------------------------------------------
##### Create physic agent: orocos task
import xdefw.rtt
import deploy.deployer as ddeployer
phy = xdefw.rtt.Task(ddeployer.load("physic", "xdefw::physics::PhysicAgent", "xdefw-agt-physics"),
binding_class=xdefw.rtt.ObjectStringBinding,
static_classes=['agent', 'Connectors'])
main_scene = phy.s.GVM.Scene.new("main_scene") # create a physical scene
lmd = phy.s.XCD.Scene.new_LMD("lmd", 0.05, 3*3.14) # create a collision scene using
# the Local Minimum Distance algorithm
##### Configure main scene
period = 0.01
main_scene.setIntegratorFlags(17) # 17 = 1 + 16 : dynamic + gauss seidel solver
main_scene.setUcRelaxationFactor(1.0) # relaxation factor
main_scene.setFdvrFactor(0.2) #
main_scene.setGeometricalScene(lmd) # link collision and physic scene