def add_iCub(damping = 2, H_init=None, fixed_base=True):
if H_init is None:
H_init = lgsm.Displacement()
H_init = lgsm.Displacement(H_init)
world = desc.scene.createWorld()
bodies_data = XDEiCub.get_bodies_data()
kin_tree = XDEiCub.get_kinematic_tree(damping)
########## Build physical scene
print "add iCub Kinematic Tree"
desc.physic.fillKinematicTree(world.scene.physical_scene.nodes.add(),
tree=kin_tree,
fixed_base=fixed_base,
H_init=H_init)
print "add iCub Mechanism"
desc.physic.addMechanism(world.scene.physical_scene,
name="iCub",
root_node="waist",
trim_nodes=[],
bodies=list(bodies_data),
segments=list(bodies_data))
phy_root = desc.physic.findInPhysicalScene(world.scene.physical_scene, "waist")
def setNodeMomentsOfInertia(node):
b_name = node.rigid_body.name
MoI = bodies_data[b_name][1]
node.rigid_body.moments_of_inertia.extend(MoI)
desc.core.visitDepthFirst(setNodeMomentsOfInertia, phy_root)
def setNodeMaterial(node):
node.rigid_body.contact_material = "material.metal"
desc.core.visitDepthFirst(setNodeMaterial, phy_root)
return world
