def add_snake(w,
nbody,
lengths=None,
masses=None,
gpos=None,
gvel=None,
is_fixed=True):
"""Add a snake-shape robot to the world.
**Example:**
>>> w = World()
>>> add_snake(w, 3, lengths=[1.5, 1., 5.])
"""
assert isinstance(w, World)
if lengths is None:
lengths = [.5] * nbody
assert nbody == len(lengths)
if masses is None:
masses = [2.] * nbody
assert nbody == len(masses)
if gpos is None:
gpos = [0.] * nbody
assert nbody == len(gpos)
if gvel is None:
gvel = [0.] * nbody
assert nbody == len(gvel)
if is_fixed:
frame = w.ground
else:
L = lengths[0] / 2.
body = Body(mass=box([L, L, L], masses[0]))
w.add_link(w.ground, FreeJoint(), body)
frame = body
for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
radius = length / 10. #TODO use 5 instead of 10
#angle = pi/2.
angle = 0. #TODO: remove, it is here for testing
body = Body(
mass=transport(cylinder(length, radius, mass),
dot(rotx(angle), transl(0., -length / 2., 0.))))
joint = RzJoint(gpos=q, gvel=dq)
w.add_link(frame, joint, body)
frame = SubFrame(body, transl(0., length, 0.))
w.register(frame)
w.init()
def add_cylinder(world, length=1., radius=1., mass=1., name='Cylinder'):
"""Build a cylinder robot, whose symmetry axis is along the z-axis.
Example:
>>> w = World()
>>> add_cylinder(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
cylinder = Body(name=name, mass=massmatrix.cylinder(length, radius, mass))
world.add_link(world.ground, FreeJoint(), cylinder)
world.register(Cylinder(cylinder, length, radius))
world.init()
def add_box(world, half_extents=(1., 1., 1.), mass=1., name='Box'):
"""Build a box robot.
Example:
>>> w = World()
>>> add_box(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
box = Body(name=name, mass=massmatrix.box(half_extents, mass))
world.add_link(world.ground, FreeJoint(), box)
world.register(Box(box, half_extents))
world.init()
def add_sphere(world, radius=1., mass=1., name=None):
"""Build a ball robot.
Example:
>>> w = World()
>>> add_sphere(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
ball = Body(name=name, mass=massmatrix.sphere(radius, mass))
world.add_link(world.ground, FreeJoint(), ball)
world.register(Sphere(ball, radius))
world.init()
def runTest(self):
b0 = Body(mass=eye(6))
w = World()
w.add_link(w.ground, FreeJoint(), b0)
w.init()
ctrl = WeightController()
w.register(ctrl)
c0 = BallAndSocketConstraint(frames=(w.ground, b0))
w.register(c0)
w.init()
w.update_dynamic()
dt = 0.001
w.update_controllers(dt)
w.update_constraints(dt)
self.assertListsAlmostEqual(c0._force, [0., 9.81, 0.])
w.integrate(dt)
w.update_dynamic()
self.assertListsAlmostEqual(
b0.pose, [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
def add(w, is_fixed=False, create_shapes=True, create_contacts=True):
"""
construction of the icub robot for arboris-python:
Kinematics data are from: http://eris.liralab.it/wiki/ICubForwardKinematics
Inertia comes from the Icub.cpp used in the iCub_SIM program
Some data are not well explained, or are badly defined
"""
bodies_data = get_bodies_data()
bodies_shapes_data = get_bodies_shapes_data()
joints_data = get_joints_data()
shapes_data = get_contact_data()
## bodies creation
bodies = {}
for name, data in bodies_data.items():
bodies[name] = Body(name=name)
mass = zeros((6, 6))
for dims, m, H in data: # get dims, mass and transformation from data
sf = SubFrame(bodies[name], H)
if len(dims) == 3: # check the type of shape: len =3: box
M = box(dims, m)
elif len(dims) == 2: # len =2:cylinder,
M = cylinder(dims[0], dims[1], m)
elif len(dims) == 1: # len =1:sphere,
M = sphere(dims[0], m)
else:
raise ValueError
mass += transport(M, inv(H)) # add the mass of the shape to
bodies[name].mass = mass # the total mass
## check if iCub has its waist fixed on the structure (the ground)
if is_fixed:
bodies['waist'] = w.ground
else:
w.add_link(w.ground, FreeJoint(name='root'), bodies['waist'])
## joints creation
for name, data in joints_data.items():
parent, Hp_l, child = data
w.add_link(SubFrame(bodies[parent], Hp_l), RzJoint(name=name),
bodies[child])
if create_shapes is True:
## body shapes creations
for name, data in bodies_shapes_data.items():
for dims, H in data: # get dims, mass and transformation from data
sf = SubFrame(bodies[name], H)
if len(dims) == 3: # check the type of shape: len =3: box
sh = Box(sf, dims, name + ".body_shape")
elif len(dims) == 2: # len =2:cylinder,
sh = Cylinder(sf, dims[0], dims[1], name + ".body_shape")
elif len(dims) == 1: # len =1:sphere,
sh = Sphere(sf, dims[0], name + ".body_shape")
else:
raise ValueError
w.register(sh)
if create_contacts is True:
## contact shapes creation
for name, data in shapes_data.items():
parent, dims, Hpf = data
sf = SubFrame(bodies[parent], Hpf, name=name)
if len(dims) == 3: # check the type of shape: len =3: box
sh = Box(sf, dims, name=name)
elif len(dims) == 2: # len =2:cylinder,
sh = Cylinder(sf, dims[0], dims[1], name=name)
elif len(dims) == 1: # len =1:sphere,
sh = Sphere(sf, dims[0], name=name)
else:
sh = Point(sf, name=name)
w.register(sh)
w.init()
H_bc = transl(1, 1, 1)
else:
H_bc = eye(4)
half_extents = (.5, .5, .5)
mass = 1.
body = Body(name='box_body',
mass=massmatrix.transport(massmatrix.box(half_extents, mass),
H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0., 0., 0., 0., 0., 0.])
else:
twist_c = array([1, 1, 1, 0, 0, 0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))
weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
from arboris.visu_osg import Drawer
timeline = arange(0, 1, 5e-3)
simulate(w, timeline, [obs, Drawer(w)])
time = timeline[:-1]
obs.plot_error()
show()
adjacency = j.adjacency # ..............: iadjacency
dadjoint = j.dadjoint # ..............: idadjoint
gpos = j.gpos # generalized coordinates in joint subspace
gvel = j.gvel # generalized velocity ...
ndof = j.ndof # number of DoF
dof = j.dof # slice of the DoF in the world instance
jacobian = j.jacobian # inner Jacobian
djacobian = j.djacobian # inner time derivative Jacobian
# Example on how to instanciate a new joint
from arboris.joints import RyRxJoint, FreeJoint #there are many others
j = RyRxJoint(gpos=[.1, .2], gvel=[.0, .1], name="RyRxj")
j = FreeJoint(gpos=transl(.1, .2, .3),
gvel=[.1, .2, .3, .4, .5, .6],
name="freej")
##### The Shape class ##########################################################
# Just some shapes for scene decoration or for collision detection.
from arboris.shapes import Point, Plane, Sphere, Cylinder, Box
f = frames["EndEffector"]
s0 = Point(frame=f, name="thePoint")
s1 = Plane(frame=f, coeffs=(0, 1, 0, .1), name="thePlane")
s2 = Sphere(frame=f, radius=.1, name="theSphere")
s3 = Cylinder(frame=f, length=1., radius=.1,
name="theCylinder") # along z axis
s4 = Box(frame=f, half_extents=(.1, .2, .3), name="theBox")
def add_human36(world, height=1.741, mass=73, anat_lengths=None, prefix=''):
"""Add an anthropometric humanoid model to the world.
:param height: the human height in meters. Ignored if ``anat_lengths``
is provided.
:type height: float
:param mass: the human mass in kilograms.
:type mass: float
:param anat_lengths: the human anatomical lengths.
Computed from ``height`` if not provided
:type anat_lengths: dict
:param prefix: name of the human, used to prefix every object name.
:type prefix: string
:param return_lists: if True, returns of a tuple of the added objects
:return: None
**Examples**
>>> w = World()
>>> # add a normal human
>>> add_human36(w, height=1.8, prefix="Bob's ")
>>> # add a human with a shorter left arm
>>> L = anat_lengths_from_height(1.8)
>>> L['yhumerusL'] *= .7
>>> L['yforearmL'] *= .7
>>> L['yhandL'] *= .7
>>> add_human36(w, anat_lengths=L, prefix="Casimodo's ")
>>> frames = w.getframes()
>>>
>>> frames["Bob's Left stylion"].bpose[0:3, 3]
>>> frames["Bob's Left stylion"].pose[0:3, 3]
>>> frames["Casimodo's Left stylion"].bpose[0:3, 3]
"""
assert isinstance(world, World)
w = world
if anat_lengths is None:
L = anat_lengths_from_height(height)
else:
L = anat_lengths
h = height_from_anat_lengths(L)
# create the bodies
bodies = NamedObjectsList()
def add_body(name, mass, com_position, gyration_radius):
#mass matrix at com
mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1))))
H_fg = eye(4)
H_fg[0:3, 3] = com_position
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(Hg.adjoint(H_gf).T, dot(mass_g, Hg.adjoint(H_gf)))
bodies.append(Body(name=prefix + name, mass=mass_o))
# Lower Part of Trunk
add_body("LPT", 0.275 * mass, [0, 0.5108 * L['yvT10'], 0],
array([0.2722, 0.2628, 0.226]) * L['yvT10'])
add_body("ThighR", 0.1416 * mass, [0, -0.4095 * L['yfemurR'], 0],
array([0.329, 0.149, 0.329]) * L['yfemurR'])
add_body("ShankR", 0.0433 * mass, [0, -0.4459 * L['ytibiaR'], 0],
array([0.255, 0.103, 0.249]) * L['ytibiaR'])
add_body("FootR", 0.0137 * mass,
[0.4415 * L['xfootR'] - L['xheelR'], -L['yfootR'] / 2., 0.],
array([0.124, 0.257, 0.245]) * L['xfootR'])
add_body("ThighL", 0.1416 * mass, [0, -0.4095 * L['yfemurL'], 0],
array([0.329, 0.149, 0.329]) * L['yfemurL'])
add_body("ShankL", 0.0433 * mass, [0, -0.4459 * L['ytibiaL'], 0],
array([0.255, 0.103, 0.249]) * L['ytibiaL'])
add_body("FootL", 0.0137 * mass,
[0.4415 * L['xfootL'] - L['xheelL'], -L["yfootL"] / 2, 0.],
array([0.124, 0.257, 0.245]) * L['xfootL'])
# Upper Part of Trunk
add_body("UPT", 0.1596 * mass,
[(L['xsternoclavR'] + L['xsternoclavL']) / 4., 0.7001 *
(L['ysternoclavR'] + L['ysternoclavL']) / 2., 0.],
array([0.716, 0.659, 0.454]) * L['ysternoclavR'])
# right shoulder
add_body("ScapulaR", 0., [0., 0., 0.], array([0., 0., 0.]))
add_body("ArmR", 0.0271 * mass, [0., -0.5772 * L['yhumerusR'], 0.],
array([0.285, 0.158, 0.269]) * L['yhumerusR'])
add_body("ForearmR", 0.0162 * mass, [0., -0.4574 * L['yforearmR'], 0.],
array([0.276, 0.121, 0.265]) * L['yforearmR'])
add_body("HandR", 0.0061 * mass, [0, -0.3691 * L['yhandR'], 0],
array([0.235, 0.184, 0.288]) * L['yhandR'])
# left shoulder
add_body("ScapulaL", 0., [0., 0., 0.], array([0., 0., 0.]))
add_body("ArmL", 0.0271 * mass, [0, -0.5772 * L['yhumerusL'], 0.],
array([0.285, 0.158, 0.269]) * L['yhumerusL'])
add_body("ForearmL", 0.0162 * mass, [0, -0.4574 * L['yforearmL'], 0],
array([0.276, 0.121, 0.265]) * L['yforearmL'])
add_body("HandL", 0.0061 * mass, [0, -0.3691 * L['yhandL'], 0],
array([0.288, 0.184, 0.235]) * L['yhandL'])
add_body("Head", 0.0694 * mass, [0, 0.4998 * L['yhead'], 0],
array([0.303, 0.261, 0.315]) * L['yhead'])
# create the joints and add the links (i.e. joints+bodies)
def add_link(body0, transl, joint, name, body1):
if not isinstance(body0, Body):
body0 = bodies[prefix + body0]
frame0 = SubFrame(body0, Hg.transl(*transl))
joint.name = prefix + name
w.add_link(frame0, joint, bodies[prefix + body1])
add_link(w.ground, (0, L['yfootL'] + L['ytibiaL'] + L['yfemurL'], 0),
FreeJoint(), 'Root', 'LPT')
add_link('LPT', (0, 0, L['zhip'] / 2.), RzRyRxJoint(), 'HipR', 'ThighR')
add_link('ThighR', (0, -L['yfemurR'], 0), RzJoint(), 'KneeR', 'ShankR')
add_link('ShankR', (0, -L['ytibiaR'], 0), RzRxJoint(), 'AnkleR', 'FootR')
add_link('LPT', (0, 0, -L['zhip'] / 2.), RzRyRxJoint(), 'HipL', 'ThighL')
add_link('ThighL', (0, -L['yfemurL'], 0), RzJoint(), 'KneeL', 'ShankL')
add_link('ShankL', (0, -L['ytibiaL'], 0), RzRxJoint(), 'AnkleL', 'FootL')
add_link('LPT', (-L['xvT10'], L['yvT10'], 0), RzRyRxJoint(), 'Back', 'UPT')
add_link('UPT', (L['xsternoclavR'], L['ysternoclavR'], L['zsternoclavR']),
RyRxJoint(), 'Torso-ScapR', 'ScapulaR')
add_link('ScapulaR', (-L['xshoulderR'], L['yshoulderR'], L['zshoulderR']),
RzRyRxJoint(), 'Arm-ScapR', 'ArmR')
add_link('ArmR', (0, -L['yhumerusR'], 0), RzRyJoint(), 'ElbowR',
'ForearmR')
add_link('ForearmR', (0, -L['yforearmR'], 0), RzRxJoint(), 'WristR',
'HandR')
add_link('UPT', (L['xsternoclavL'], L['ysternoclavL'], -L['zsternoclavL']),
RyRxJoint(), 'Torso-ScapL', 'ScapulaL')
add_link('ScapulaL', (-L['xshoulderL'], L['yshoulderL'], -L['zshoulderL']),
RzRyRxJoint(), 'Arm-ScapL', 'ArmL')
add_link('ArmL', (0, -L['yhumerusL'], 0), RzRyJoint(), 'ElbowL',
'ForearmL')
add_link('ForearmL', (0, -L['yforearmL'], 0), RzRxJoint(), 'WristL',
'HandL')
add_link('UPT', (L['xvT10'], L['yvC7'], 0), RzRyRxJoint(), 'Neck', 'Head')
# add the tags
tags = NamedObjectsList()
def add_tag(name, body, position):
"""Returns data about anatomical landmarks as defined in HuMAnS.
:param name: name of the tag (copied from HuMAnS)
:param body: name of the body in whose frame the point is defined
:position: tag coordinates in meters
"""
if name:
name = prefix + name
tag = SubFrame(bodies[prefix + body], Hg.transl(*position), name)
tags.append(tag)
w.register(tag)
# we add 1e-4*h to keep the compatibility with HuMAnS:
add_tag('Right foot toe tip', 'FootR',
[L['xfootR'] - L['xheelR'] + 1e-4 * h, -L['yfootR'], 0.])
add_tag('Right foot heel', 'FootR', [-L['xheelR'], -L['yfootR'], 0.])
add_tag('Right foot phalange 5', 'FootR',
[0.0662 * h, -L['yfootR'], 0.0305 * h])
add_tag('Right foot phalange 1', 'FootR',
[0.0662 * h, -L['yfootR'], -0.0305 * h])
add_tag('Right foot lateral malleolus', "ShankR",
[0., -L['ytibiaR'], 0.0249 * h])
add_tag('Femoral lateral epicondyle', "ThighR",
[0., -L['yfemurR'], 0.0290 * h])
add_tag('Right great trochanter', "ThighR",
[0., 0., 0.0941 * h - L['zhip'] / 2.])
add_tag('Right iliac crest', "LPT", [0.0271 * h, 0.0366 * h, 0.0697 * h])
# we add 1e-4*h to keep the compatibility with HuMAnS:
add_tag('Left foot toe tip', "FootL",
[L['xfootL'] - L['xheelL'] + 1e-4 * h, -L['yfootL'], 0.])
add_tag('Left foot heel', "FootL", [-L['xheelL'], -L['yfootL'], 0.])
add_tag('Left foot phalange 5', "FootL",
[0.0662 * h, -L['yfootL'], -0.0305 * h])
add_tag('Left foot phalange 1', "FootL",
[0.0662 * h, -L['yfootL'], 0.0305 * h])
add_tag('Left foot lateral malleolus', "ShankL",
[0, -L['ytibiaL'], -0.0249 * h])
add_tag('Left femoral lateral epicondyle', "ThighL",
[0, -L['yfemurL'], -0.0290 * h])
add_tag('Left great trochanter', "ThighL",
[0, 0, -0.0941 * h + L['zhip'] / 2.])
add_tag('Left iliac crest', "LPT", [0.0271 * h, 0.0366 * h, -0.0697 * h])
add_tag('Substernale (Xyphoid)', "UPT", [0.1219 * h, 0, 0])
add_tag('Suprasternale', "UPT",
[(L['xsternoclavL'] + L['xsternoclavL']) / 2.,
(L['ysternoclavL'] + L['ysternoclavL']) / 2., 0])
add_tag('Right acromion', "ScapulaR",
[-L['xshoulderR'], 0.0198 * h + L['yshoulderR'], L['zshoulderR']])
add_tag('Right humeral lateral epicondyle (radiale)', "ArmR",
[0., -L['yhumerusR'], 0.0211 * h])
add_tag('Right stylion', "ForearmR", [0., -0.1533 * h, 0.0331 * h])
add_tag('Right 3rd dactylion', "HandR", [0., -L['yhandR'], 0.])
add_tag('Left acromion', "ScapulaL",
[-L['xshoulderL'], 0.0198 * h + L['yshoulderL'], -L['zshoulderL']])
add_tag('Left humeral lateral epicondyle (radiale)', "ArmL",
[0., -L['yhumerusL'], -0.0211 * h])
add_tag('Left stylion', "ForearmL", [0., -0.1533 * h, -0.0331 * h])
add_tag('Left 3rd dactylion', "HandL", [0., -L['yhandL'], 0.])
add_tag('Cervicale', "UPT", [-0.0392 * 0. + L['xvT10'], L['yvC7'], 0.])
add_tag('Vertex', "Head", [0., L["yhead"], 0.])
# Add point shapes to the feet
for k in ('Right foot toe tip', 'Right foot heel', 'Right foot phalange 5',
'Right foot phalange 1', 'Left foot toe tip', 'Left foot heel',
'Left foot phalange 5', 'Left foot phalange 1'):
name = prefix + k
shape = Point(tags[name], name=name)
w.register(shape)
w.init()
return None