def create_icub_and_init(chair=False, gravity=False):
## CREATE THE WORLD
w = World()
w._up[:] = [0, 0, 1]
icub.add(w, create_shapes=False)
w.register(Plane(w.ground, (0, 0, 1, 0), "floor"))
if chair is True:
w.register(Sphere(w.getbodies()['l_hip_2'], .0325, name='l_gluteal'))
w.register(Sphere(w.getbodies()['r_hip_2'], .0325, name='r_gluteal'))
w.register(
Box(SubFrame(w.ground, transl(.2, 0, 0.26)), (.075, .1, .02),
name='chair'))
w.register(
Box(SubFrame(w.ground, transl(.255, 0, 0.36)), (.02, .1, .1),
name='chair_back'))
## INIT
joints = w.getjoints()
joints['root'].gpos[0:3, 3] = [0, 0, .598]
joints['l_shoulder_roll'].gpos[:] = pi / 8
joints['r_shoulder_roll'].gpos[:] = pi / 8
joints['l_elbow_pitch'].gpos[:] = pi / 8
joints['r_elbow_pitch'].gpos[:] = pi / 8
joints['l_knee'].gpos[:] = -0.1
joints['r_knee'].gpos[:] = -0.1
joints['l_ankle_pitch'].gpos[:] = -0.1
joints['r_ankle_pitch'].gpos[:] = -0.1
shapes = w.getshapes()
floor_const = [
SoftFingerContact((shapes[s], shapes['floor']), 1.5, name=s)
for s in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']
]
for c in floor_const:
w.register(c)
if chair is True:
chair_const = [
SoftFingerContact((shapes[s], shapes['chair']), 1.5, name=s)
for s in ['l_gluteal', 'r_gluteal']
]
for c in chair_const:
w.register(c)
w.update_dynamic()
## CTRL
if gravity:
w.register(WeightController())
return w
def create_icub_and_init(chair=False, gravity=False):
## CREATE THE WORLD
w = World()
w._up[:] = [0,0,1]
icub.add(w, create_shapes=False)
w.register(Plane(w.ground, (0,0,1,0), "floor"))
if chair is True:
w.register(Sphere(w.getbodies()['l_hip_2'], .0325, name='l_gluteal'))
w.register(Sphere(w.getbodies()['r_hip_2'], .0325, name='r_gluteal'))
w.register(Box(SubFrame(w.ground, transl(.2, 0, 0.26 )), (.075, .1, .02), name='chair'))
w.register(Box(SubFrame(w.ground, transl(.255, 0, 0.36 )), (.02, .1, .1), name='chair_back'))
## INIT
joints = w.getjoints()
joints['root'].gpos[0:3,3] = [0,0,.598]
joints['l_shoulder_roll'].gpos[:] = pi/8
joints['r_shoulder_roll'].gpos[:] = pi/8
joints['l_elbow_pitch'].gpos[:] = pi/8
joints['r_elbow_pitch'].gpos[:] = pi/8
joints['l_knee'].gpos[:] = -0.1
joints['r_knee'].gpos[:] = -0.1
joints['l_ankle_pitch'].gpos[:] = -0.1
joints['r_ankle_pitch'].gpos[:] = -0.1
shapes = w.getshapes()
floor_const = [SoftFingerContact((shapes[s], shapes['floor']), 1.5, name=s)for s in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']]
for c in floor_const:
w.register(c)
if chair is True:
chair_const = [SoftFingerContact((shapes[s], shapes['chair']), 1.5, name=s)for s in ['l_gluteal', 'r_gluteal']]
for c in chair_const:
w.register(c)
w.update_dynamic()
## CTRL
if gravity:
w.register(WeightController())
return w
It presents the World, Body, Joint, SubFrame and Constraint classes.
"""
##### About the World ##########################################################
from arboris.core import World
from arboris.robots import simplearm
w = World() # create an instance of world.
simplearm.add_simplearm(w) # add a simple arm robot
# The world is composed of ...
bodies = w.getbodies() # ... named list of bodies
joints = w.getjoints() # ... named list of joints
frames = w.getframes() # ... named list of frames (including bodies)
shapes = w.getshapes() # ... named list of shapes
consts = w.getconstraints() # ... named list of constraints
ctrls = w.getcontrollers() # ... named list of controllers
# And that's all!
print("bodies", bodies)
print("joints", joints)
# These lists are named, so instances can be retrieved as follows:
body_Arm = bodies["Arm"]
joint_Shoulder = joints["Shoulder"]
print("Arm", body_Arm)
print("Shoulder", joint_Shoulder)
# At the beginning, the world has one body corresponding to the galilean frame
galilean_frame = w.ground
w = World() from arboris.robots import human36 human36.add_human36(w) w.update_dynamic() ######################################################## ## INIT ######################################################## joints = w.getjoints() bodies = w.getbodies() frames = w.getframes() shapes = w.getshapes() joints[0].gpos[0:3,3] = array([.1, .2, .3]) w.update_dynamic() ######################################################## ## CONTROLLERS ######################################################## from arboris.controllers import WeightController w.register(WeightController()) ########################################################
## WORLD BUILDING ######################################################## w = World() from arboris.robots import human36 human36.add_human36(w) w.update_dynamic() ######################################################## ## INIT ######################################################## joints = w.getjoints() bodies = w.getbodies() frames = w.getframes() shapes = w.getshapes() joints[0].gpos[0:3, 3] = array([.1, .2, .3]) w.update_dynamic() ######################################################## ## CONTROLLERS ######################################################## from arboris.controllers import WeightController w.register(WeightController()) ######################################################## ## OBSERVERS ######################################################## obs = []
"""
##### About the World ##########################################################
from arboris.core import World
from arboris.robots import simplearm
w = World() # create an instance of world.
simplearm.add_simplearm(w) # add a simple arm robot
# The world is composed of ...
bodies = w.getbodies() # ... named list of bodies
joints = w.getjoints() # ... named list of joints
frames = w.getframes() # ... named list of frames (including bodies)
shapes = w.getshapes() # ... named list of shapes
consts = w.getconstraints() # ... named list of constraints
ctrls = w.getcontrollers() # ... named list of controllers
# And that's all!
print("bodies", bodies)
print("joints", joints)
# These lists are named, so instances can be retrieved as follows:
body_Arm = bodies["Arm"]
joint_Shoulder = joints["Shoulder"]
print("Arm", body_Arm)
print("Shoulder", joint_Shoulder)
# At the beginning, the world has one body corresponding to the galilean frame
galilean_frame = w.ground
