def add_kuka_with_meshes(damping=0, kuka_offset=0.001):
current_path = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
urdfworld = xrl.createWorldFromUrdfFile(
xr.kuka, "kuka", [0.0, 0.0, 0.0, 1, 0, 0, 0], True, damping, 0.005
) #, "material.concrete") DEFINE THE POSITION AND ORIENTATION OF ROBOT IN WORLD //Damping Ori 0.005
#urdfworld = xrl.createWorldFromUrdfFile(xr.kuka, "kuka", [0.0,0.0, 0.0, 0, 0, 0, 1], True, damping, 0.005) #, "material.concrete") DEFINE THE POSITION AND ORIENTATION OF ROBOT IN WORLD
return urdfworld
def addObst_1(world):
obst_1_world = xrl.createWorldFromUrdfFile("resources/urdf/env11.xml",
"obst_1",
[0.1, 0.35, 0.0, 1, 0, 0, 0],
True, 0.1, 0.01)
phy_obst_1_world = desc.simple.scene.parseColladaFile(
"resources/dae/env11.dae",
append_label_library=".phyobst_1",
append_label_graph_meshes=".obst_1")
desc.simple.graphic.addGraphicalTree(world,
obst_1_world,
node_name="obst_1")
shpere_node = desc.core.findInTree(
obst_1_world.scene.graphical_scene.root_node, "node-obst_1")
comp_obst_1 = desc.simple.collision.addCompositeMesh(
world,
phy_obst_1_world,
composite_name="obst_1.comp",
offset=0.00,
clean_meshes=False,
ignore_library_conflicts=False)
#desc.collision.copyFromGraphicalTree(comp_obst_1.root_node, obst_1_node)
desc.simple.physic.addRigidBody(world,
"obst_1",
mass=0.00001,
contact_material="material.concrete")
#obst_1_position = lgsm.Displacementd(0.60,0.0,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) posi ori paper Ec
#obst_1_position = lgsm.Displacementd(-0.2,2.0,0.256, 0.7071067811865476, 0.7071067811865476, 0, 0) #(0.1,0.3,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) //Cette ligne a ete utilise pour table horizontale
#obst_1_position = lgsm.Displacementd(0.18,0.35,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) #(0.1,0.3,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) //Cette ligne a ete utilise pour table horizontale
obst_1_position = lgsm.Displacementd(
0.18, 0.19, 0.0, 0.7071067811865476, 0, 0, 0.7071067811865475
) #(0.1,0.3,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) //Cette ligne a ete utilise pour table horizontale
#obst_1_position.setTranslation(lgsm.vector(0.9,0.0,0.0))
desc.simple.physic.addFreeJoint(world, "obst_1.joint", "obst_1",
obst_1_position)
#Binding graph, phy and coll object
obst_1_graph_node = desc.core.findInTree(
world.scene.graphical_scene.root_node, "obst_1")
obst_1_phy_node = desc.physic.findInPhysicalScene(
world.scene.physical_scene, "obst_1")
obst_1_graph_node.name = "obst_1" # it is suitable to have the same name for both graphics and physics.
obst_1_phy_node.rigid_body.composite_name = "obst_1.comp"
def addGround(world):
current_path = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
ground_world = xrl.createWorldFromUrdfFile(
"resources/urdf/ground.xml", "ground",
[0.0, 0.0, 0.0, 0.7071067811865476, 0, 0, 0.7071067811865475], False,
0.1, 0.01) # , "material.concrete")
phy_ground_world = desc.simple.scene.parseColladaFile(
"resources/dae/ground_phy_50mm.dae",
append_label_library=".phyground",
append_label_graph_meshes=".ground_50mm")
desc.simple.graphic.addGraphicalTree(world,
ground_world,
node_name="ground")
ground_node = desc.core.findInTree(
ground_world.scene.graphical_scene.root_node, "node-ground")
comp_ground = desc.simple.collision.addCompositeMesh(
world,
phy_ground_world,
composite_name="ground.comp",
offset=0.00,
clean_meshes=False,
ignore_library_conflicts=False)
#desc.collision.copyFromGraphicalTree(comp_ground.root_node, ground_node)
desc.simple.physic.addRigidBody(world,
"ground",
mass=100000000,
contact_material="material.concrete")
#ground_position = lgsm.Displacementd(-0.2,2.0,0.256, 0.7071067811865476, 0.7071067811865476, 0, 0) #(0.1,0.3,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) //Ligne pour table horizontale
ground_position = lgsm.Displacementd(
0.1, 0.3, 0.0, 0.7071067811865476, 0, 0, 0.7071067811865475
) #(0.1,0.3,0.0, 0.7071067811865476, 0, 0, 0.7071067811865475) //Pour mur vertical
desc.simple.physic.addFixedJoint(world, "ground.joint", "ground",
ground_position)
#Binding graph, phy and coll object
ground_graph_node = desc.core.findInTree(
world.scene.graphical_scene.root_node, "ground")
ground_phy_node = desc.physic.findInPhysicalScene(
world.scene.physical_scene, "ground")
ground_graph_node.name = "ground" # it is suitable to have the same name for both graphics and physics.
ground_phy_node.rigid_body.composite_name = "ground.comp"
def addObst_2(world):
obst_2_world = xrl.createWorldFromUrdfFile("resources/urdf/env12.xml",
"obst_2",
[0.6, -0.2, 0.0, 1, 0, 0, 0],
True, 0.1, 0.01)
phy_obst_2_world = desc.simple.scene.parseColladaFile(
"resources/dae/env12.dae",
append_label_library=".phyobst_2",
append_label_graph_meshes=".obst_2")
desc.simple.graphic.addGraphicalTree(world,
obst_2_world,
node_name="obst_2")
shpere_node = desc.core.findInTree(
obst_2_world.scene.graphical_scene.root_node, "node-obst_2")
comp_obst_2 = desc.simple.collision.addCompositeMesh(
world,
phy_obst_2_world,
composite_name="obst_2.comp",
offset=0.00,
clean_meshes=False,
ignore_library_conflicts=False)
#desc.collision.copyFromGraphicalTree(comp_obst_2.root_node, obst_2_node)
desc.simple.physic.addRigidBody(world,
"obst_2",
mass=1,
contact_material="material.concrete")
obst_2_position = lgsm.Displacementd()
#obst_2_position.setTranslation(lgsm.vector(0.6,-0.2,0.0))
obst_2_position.setTranslation(lgsm.vector(2.6, 3.2, 0.0))
desc.simple.physic.addFixedJoint(world, "obst_2.joint", "obst_2",
obst_2_position)
#Binding graph, phy and coll object
obst_2_graph_node = desc.core.findInTree(
world.scene.graphical_scene.root_node, "obst_2")
obst_2_phy_node = desc.physic.findInPhysicalScene(
world.scene.physical_scene, "obst_2")
obst_2_graph_node.name = "obst_2" # it is suitable to have the same name for both graphics and physics.
obst_2_phy_node.rigid_body.composite_name = "obst_2.comp"
def addSphere(world):
sphere_world = xrl.createWorldFromUrdfFile("resources/urdf/sphere.xml",
"sphere",
[0, 0, 0.1, 1, 0, 0, 0], False,
0.1,
0.01) # , "material.concrete")
phy_sphere_world = desc.simple.scene.parseColladaFile(
"resources/dae/sphere.dae",
append_label_library=".physphere",
append_label_graph_meshes=".sphere")
desc.simple.graphic.addGraphicalTree(world,
sphere_world,
node_name="sphere")
shpere_node = desc.core.findInTree(
sphere_world.scene.graphical_scene.root_node, "node-sphere")
comp_sphere = desc.simple.collision.addCompositeMesh(
world,
phy_sphere_world,
composite_name="sphere.comp",
offset=0.00,
clean_meshes=False,
ignore_library_conflicts=False)
#desc.collision.copyFromGraphicalTree(comp_sphere.root_node, sphere_node)
desc.simple.physic.addRigidBody(world,
"sphere",
mass=1,
contact_material="material.concrete")
sphere_position = lgsm.Displacementd()
sphere_position.setTranslation(lgsm.vector(0.7, 5.3, 5.3))
desc.simple.physic.addFixedJoint(world, "sphere.joint", "sphere",
sphere_position)
#Binding graph, phy and coll object
sphere_graph_node = desc.core.findInTree(
world.scene.graphical_scene.root_node, "sphere")
sphere_phy_node = desc.physic.findInPhysicalScene(
world.scene.physical_scene, "sphere")
sphere_graph_node.name = "sphere" # it is suitable to have the same name for both graphics and physics.
sphere_phy_node.rigid_body.composite_name = "sphere.comp"
import xde_robot_loader as xrl
import xde_resources as xr
import lgsm
import time
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.5)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
##### ROBOT
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile("resources/Robot3T.xml", rname, [0,0,0,1,0,0,0], True, 0.001, 0.001, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(False)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
robot.setJointPositions(lgsm.vector(.5,.5,.5))
dynModel.setJointPositions(lgsm.vector(.5,.5,.5))
robot.setJointVelocities(lgsm.zeros(N))
dynModel.setJointVelocities(lgsm.zeros(N))
##### SET INTERACTION
import time
import xdefw.interactive
pi = lgsm.np.pi
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.2)
wm.resizeWindow("mainWindow", 800, 600, 50, 50)
mu_sys = 0.5
##### GROUND
ground_fixed_base = True
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], ground_fixed_base, 0.001, 0.001)
wm.addWorld(groundWorld)
##### ROBOT
rname = "robot"
robot_fixed_base = False
robotWorld = xrl.createWorldFromUrdfFile(xr.romeo_collision, rname, [0,0,0.88,1,0,0,0], robot_fixed_base, 0.001, 0.001)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
#robot.enableGravity(False)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
jmap = xrl.getJointMapping(xr.romeo_collision, robot)
import sys
import os
import inspect
cpath = os.path.dirname(os.path.abspath(inspect.getfile( inspect.currentframe()))) + "/"
sys.path.append(cpath)
import rtt_interface_corba
rtt_interface_corba.Init(sys.argv)
#Creating Scene
mecha_name = "kuka2"
#Damping 100 ?
kukaWorld = xrl.createWorldFromUrdfFile(xr.kuka, mecha_name, [0.5,0.2,0.4, 1, 0, 0, 0], True, 100, 0.005) #, "material.concrete")
xrl.addContactLaws(kukaWorld)
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0.0, 1, 0, 0, 0], True, 0.1, 0.05) #, "material.concrete")
wm.addWorld(groundWorld)
wm.addWorld(kukaWorld)
#Joint position controller
import controlQ
controllerq = controlQ.createControllerQ("kukacontrollerq", TIME_STEP)
controllerq.connectToRobot(wm.phy, kukaWorld, mecha_name)
#import dummyGetQ
#dumm = dummyGetQ.createDummyGetQ("dummyget", TIME_STEP)
import deploy.deployer as ddeployer
import xde_resources as xr
import xde_robot_loader as xrl
import physicshelper
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)
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=local_minimal_distance)
wm.resizeWindow("mainWindow", 640, 480, 50, 50)
##### WORLD
## GROUND
#flat ground
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], True, 0.001, 0.001)
#uneven ground
wm.addWorld(groundWorld)
## MOVING WALL
robotWorld = xrl.createWorldFromUrdfFile("resources/moving_table.xml", "moving_wall", [-0.4,0.0,table_z,0.707,0,0.707,0], True, 0.001, 0.01)
wm.addWorld(robotWorld)
dynModel_moving_wall = xrl.getDynamicModelFromWorld(robotWorld)
## ROBOT
rname = "robot"
fixed_base = False
robotWorld = xrl.createWorldFromUrdfFile(xr.icub_simple, rname, [0,0,0.6,1,0,0,0], fixed_base, 0.001, 0.001)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
# #
###################
print "BEGIN OF SCRIPT..."
TIME_STEP = .01
wm = xwm.WorldManager()
wm.createAllAgents(TIME_STEP, phy_name="physic", create_graphic=True, graph_name = "graphic")
#######################################################################################################
print "START ALL..."
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,-0.4, 1, 0, 0, 0], True, 0.1, 0.05)
wm.addWorld(groundWorld)
kukaWorld = xrl.createWorldFromUrdfFile(xr.kuka, "kuka", [0,0,-0.0, 0.707,0, 0.707, 0], True, 0.5, 0.01)
wm.addWorld(kukaWorld)
kuka = wm.phy.s.GVM.Robot("kuka")
rx90World = xrl.createWorldFromUrdfFile(xr.rx90, "rx90", [-0.5,0,0, 1, 0, 0, 0], True, 0.5, 0.01)
wm.addWorld(rx90World)
rx90 = wm.phy.s.GVM.Robot("rx90")
dummyWorld = xrl.createWorldFromUrdfFile("resources/urdf/dummy2.xml", "dummy", [0,0,.5, 1, 0, 0, 0], True, 0.5, 0.01)
wm.addWorld(dummyWorld,)
dummy = wm.phy.s.GVM.Robot("dummy")
import xde_robot_loader as xrl
import xde_resources as xr
import lgsm
import time
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.2)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
##### ROBOT
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile(xr.kuka, rname, [0,0,0,1,0,0,0], True, 0.001, 0.01, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
##### CTRL
import xde_isir_controller as xic
ctrl = xic.ISIRController(dynModel, rname, wm.phy, wm.icsync, "quadprog", True)
jointConst = ctrl.add_constraint( xic.JointLimitConstraint(ctrl.getModel(), -0.5 * lgsm.ones(N), 0.8 * lgsm.ones(N), .1) )
#jointConst.setJointLimits(-0.5 * lgsm.ones(N), 0.8 * lgsm.ones(N))
H_initPos = lgsm.Displacement(initPos[0], initPos[1], initPos[2], np.cos(initRot/2), 0, 0, np.sin(initRot/2))
####################
## Create agents ##
####################
dt = .01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.01, uc_relaxation_factor=0.01)
###################
## Create ground ##
###################
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], True, 0.001, 0.001)
wm.addWorld(groundWorld)
####################
## Create manikin ##
####################
# configure manikin morphology
NAME = 'Pauline'
people = {'Pauline':[1.56, 53.]}
size, mass = people[NAME]
# Create manikin
import desc.simple.scene, human, scene
vhWorld = desc.simple.scene.createWorld()
vhName = "manikin"
##### GROUND
#minimal damping = damping dans les joints pour stabiliser la simulation
#il est fixe - il devrait pas changer la simu pour le ground
#composite = few inches added to have a continuous mesh on the ground (for contacts to have the normal)
#uneven ground
groundWorld = xrl.createWorldFromDae("resources/uneven_surface.dae", "ground", [0,0,0.52,1,0,0,0], True, True, None, 0.001, 1.0, 1.0, None, None, 'material.concrete')
wm.addWorld(groundWorld)
##### ROBOT
rname = "robot"
fixed_base = False
robotWorld = xrl.createWorldFromUrdfFile(xr.icub_simple, rname, [0,0,0.6,1,0,0,0], fixed_base, 0.001, 0.001)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
# NEW WAY: joint map of the robot
jmap = xrl.getJointMapping(xr.icub_simple, robot)
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, 1.5)
robot.enableContactWithBody("ground", True)
#icub = xr.icub_ge03_iden_visc_friction
#icub = xr.icub_ge03_no_visc_friction
robot_name = "robot"
robot1_name = "robot_comp_visc"
robot2_name = "robot_iden_visc"
robot3_name = "robot_no_visc"
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.2)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
### GROUND
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], True, 0.001, 0.001)
wm.addWorld(groundWorld)
### ROBOT
#robot floating
robotWorld = xrl.createWorldFromUrdfFile(icub, robot_name, [x0,y0,z0,1,0,0,0], fixed_base, minimal_damping, composite_offset)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(robot_name)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
jmap = xrl.getJointMapping(xr.icub_simple, robot)
print "JOINT MAP"
print jmap
import xde_robot_loader as xrl
import xde_resources as xr
import lgsm
import time
pi = lgsm.np.pi
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.2)
wm.resizeWindow("mainWindow", 640, 480, 200, 50)
##### GROUND
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], True, 0.001, 0.001)
wm.addWorld(groundWorld)
##### ROBOT
rname = "robot"
fixed_base = False
#robotWorld = xrl.createWorldFromUrdfFile("urdf/walker.xml", rname, [0,0,0.95,1,0,0,0], fixed_base, 0.001, 0.001)
robotWorld = xrl.createWorldFromUrdfFile("urdf/walker.xml", rname, [0,0,0.95,0.707,0,0,0.707], fixed_base, 0.001, 0.001)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
##
################################################################
##### AGENTS
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=local_minimal_distance, create_graphic=simulation_with_graphic_visualization)
wm.resizeWindow("mainWindow", 640, 480, 50, 50)
##### WORLD
### GROUND
if ground_flat == True: #flat ground
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", ground_flat_init_pose, True, 0.001, 0.001)
wm.addWorld(groundWorld)
environmentItems = ["ground.ground"]
elif ground_uneven == True: #uneven ground
groundWorld = xrl.createWorldFromDae("resources/uneven_surface_with_walls.dae", "ground", ground_uneven_init_pose, True, True, None, 0.001, 1.0, 1.0, None, None, 'material.concrete')
wm.addWorld(groundWorld)
environmentItems = ["ground"]
else:
print "Only ground==flat or uneven is supported for now"
exit()
### ENVIRONMENT
if world_movingWall == True: # MOVING WALL
robotWorld = xrl.createWorldFromUrdfFile("resources/moving_wall.xml", "moving_wall", [wall_x,0.2,0.85,1,0,0,0], True, 0.001, 0.01)
wm.addWorld(robotWorld)
import xde_robot_loader as xrl
import xde_resources as xr
import lgsm
import time
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.3)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
##### ROBOT
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile(xr.kuka, rname, [0,0,0,1,0,0,0], True, 0.001, 0.001, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
##### OBSTACLES
sphereWorld = xrl.createWorldFromUrdfFile("resources/sphere.xml", "sphere1", [0.45,0,0.3,1,0,0,0], True, 0.001, 0.001)
wm.addWorld(sphereWorld)
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, 1.5)
import xde_robot_loader as xrl
import xde_resources as xr
import lgsm
import time
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.5)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
##### ROBOT
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile("resources/Robot3T.xml", rname, [0,0,0,1,0,0,0], True, 0.001, 0.001, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(False)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
##### OBSTACLES
sphereWorld = xrl.createWorldFromUrdfFile("resources/sphere.xml", "sphere1", [1.,.8,.5,1,0,0,0], True, 0.001, 0.001)
wm.addWorld(sphereWorld)
##### SET INTERACTION
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, 1.5)
import xde_resources as xr import sys import os import inspect cpath = os.path.dirname(os.path.abspath(inspect.getfile( inspect.currentframe()))) + "/" sys.path.append(cpath) import rtt_interface_corba rtt_interface_corba.Init(sys.argv) #Creating Scene mecha_name = "kuka1" kukaWorld = xrl.createWorldFromUrdfFile(xr.kuka, mecha_name, [0.5,0.2,0.4, 1, 0, 0, 0], True, 1, 0.005) #, "material.concrete") #Adding contact laws cl = kukaWorld.scene.physical_scene.contact_laws.add() cl.material_i = "material.metal" cl.material_j = "material.concrete" cl.law = cl.COULOMB cl.friction = 0.5 cl = kukaWorld.scene.physical_scene.contact_laws.add() cl.material_i = "material.metal" cl.material_j = "material.metal" cl.law = cl.COULOMB cl.friction = 0.5 groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0.0, 1, 0, 0, 0], True, 0.1, 0.05) #, "material.concrete")
# #
###################
print "BEGIN OF SCRIPT..."
TIME_STEP = .01
wm = xwm.WorldManager()
wm.createAllAgents(TIME_STEP, phy_name="physic", create_graphic=True, graph_name = "graphic")
#######################################################################################################
print "START ALL..."
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,-0.4, 1, 0, 0, 0], True, 0.1, 0.05)
wm.addWorld(groundWorld)
kukaWorld = xrl.createWorldFromUrdfFile(xr.kuka, "kuka", [0,0,-0.0, 0.707,0, 0.707, 0], True, 0.5, 0.01)
wm.addMarkers(kukaWorld)
wm.addWorld(kukaWorld)
kuka = wm.phy.s.GVM.Robot("kuka")
wm.ms.setContactLawForMaterialPair("material.metal", "material.concrete", 1, 1.0)
kuka.enableContactWithBody("ground.ground", True)
#for interactions:
wm.addInteraction([("kuka.04", "ground.ground"), ("kuka.05", "ground.ground")])
wm.removeInteraction([("kuka.04", "ground.ground"), ("kuka.05", "ground.ground")])
import xde_world_manager as xwm
import xde_robot_loader as xrl
import xde_resources as xr
import lgsm
import time
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.01)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
##### ROBOT
robotWorld = xrl.createWorldFromUrdfFile("resources/moving_wall.xml", "moving_wall", [0.6,0,0.25,1,0,0,0], True, 0.001, 0.01)
wm.addWorld(robotWorld)
dynModel_moving_wall = xrl.getDynamicModelFromWorld(robotWorld)
rname = "robot"
robotWorld = xrl.createWorldFromUrdfFile(xr.kuka, rname, [0,0,0,1,0,0,0], True, 0.001, 0.01, use_collada_color=False)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
import lgsm
import time
pi = lgsm.np.pi
##### AGENTS
dt = 0.01
wm = xwm.WorldManager()
wm.createAllAgents(dt, lmd_max=.2)
wm.resizeWindow("mainWindow", 640, 480, 1000, 50)
##### GROUND
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], True, 0.001, 0.001)
wm.addWorld(groundWorld)
##### ROBOT
rname = "robot"
fixed_base = False
robotWorld = xrl.createWorldFromUrdfFile(xr.icub_simple, rname, [0,0,0.6,1,0,0,0], fixed_base, .001, 0.001)
wm.addWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
N = robot.getJointSpaceDim()
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
jmap = xrl.getJointMapping(xr.icub_simple, robot)
import math
TIME_STEP = 0.1
M_SQRT1_2 = 1/math.sqrt(2)
wm = xwm.WorldManager()
wm.createAllAgents(TIME_STEP)
pi = lgsm.np.pi
mu_sys = 0.5
##### GROUND
ground_fixed_base = True
groundWorld = xrl.createWorldFromUrdfFile(xr.ground, "ground", [0,0,0,1,0,0,0], ground_fixed_base, 0.001, 0.001)
wm.addWorld(groundWorld)
##### ROBOT
rname = "icub"
robot_fixed_base = False
robotWorld = xrl.createWorldFromUrdfFile(xr.icub_simple, rname, [0,0,0.6,-M_SQRT1_2,0,0,M_SQRT1_2], robot_fixed_base, 0.001, 0.001)
wm.addWorld(robotWorld)
#Controller
ctrl = xdefw.rtt.Task(ddeployer.load("control", "ISIRControllerThreadXDE", "ISIRControllerThreadXDE-gnulinux", "", libdir="../../_build/"))
dynModel = xrl.getDynamicModelFromWorld(robotWorld)
robot = wm.phy.s.GVM.Robot(rname)
robot.enableGravity(True)
