def add_simplearm(world,
name='',
lengths=(0.5, 0.4, 0.2),
masses=(1.0, 0.8, 0.2),
with_shapes=False):
"""Build a planar 3-R robot.
***Example:***
>>> w = World()
>>> add_simplearm(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
def create_body(name, length, mass):
"""create a body"""
half_extents = (length / 20., length / 2., length / 20.)
mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass)
mass_matrix_at_base = arboris.massmatrix.transport(
mass_matrix_at_com, Hg.transl(0., -length / 2., 0.))
body = Body(name, mass_matrix_at_base)
if with_shapes:
f = SubFrame(body, Hg.transl(0., length / 2., 0.))
shape = arboris.shapes.Box(f, half_extents)
world.register(shape)
return body
# create the 3 bodies
arm = create_body(name + 'Arm', lengths[0], masses[0])
forearm = create_body(name + 'Forearm', lengths[1], masses[1])
hand = create_body(name + 'Hand', lengths[2], masses[2])
# create a joint between the ground and the arm
shoulder = RzJoint(name=name + 'Shoulder')
world.add_link(world.ground, shoulder, arm)
# add a frame to the arm, where the forearm will be anchored
f = SubFrame(arm, Hg.transl(0, lengths[0], 0), name + 'ElbowBaseFrame')
# create a joint between the arm and the forearm
elbow = RzJoint(name=name + 'Elbow')
world.add_link(f, elbow, forearm)
# add a frame to the forearm, where the hand will be anchored
f = SubFrame(forearm, Hg.transl(0, lengths[1], 0), name + 'WristBaseFrame')
# create a joint between the forearm and the hand
wrist = RzJoint(name=name + 'Wrist')
world.add_link(f, wrist, hand)
# create a frame at the end of the hand
f = SubFrame(hand, Hg.transl(0, lengths[2], 0), name + 'EndEffector')
world.register(f)
world.init()
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 convert_root_joint(self, world, root_joint):
assert isinstance(root_joint, Joint)
assert isinstance(world, World)
assert root_joint.frames[0].body is world.ground
if isinstance(root_joint, joints.FreeJoint):
# nothing to do
pass
else:
from arboris.homogeneousmatrix import transl
from arboris.massmatrix import box as boxmass
from arboris.shapes import Box, Sphere
from arboris.constraints import SoftFingerContact
# add a "free-floating" base body to the robot
base = Body(mass=boxmass(self.base_lengths, self.base_mass))
world.replace_joint(root_joint, root_joint.frames[0],
joints.FreeJoint(), base, base, root_joint,
root_joint.frames[1])
# add a ground plane
r = self.contact_radius
ground_half_extents = [d / 2. + 2 * r for d in self.base_lengths]
ground_plane = Box(world.ground, ground_half_extents)
world.register(ground_plane)
# put 4 spheres at the bottom of the base
(x, y, z) = self.base_lengths
for (i, j) in ((1, 1), (1, -1), (-1, -1), (-1, 1)):
sf = SubFrame(base, transl(i * x / 2, -y / 2, j * z / 2))
sh = Sphere(sf, r)
world.register(sh)
contact = SoftFingerContact((ground_plane, sh),
self.friction_coeff)
world.register(contact)
def create_body(name, length, mass):
"""create a body"""
half_extents = (length / 20., length / 2., length / 20.)
mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass)
mass_matrix_at_base = arboris.massmatrix.transport(
mass_matrix_at_com, Hg.transl(0., -length / 2., 0.))
body = Body(name, mass_matrix_at_base)
if with_shapes:
f = SubFrame(body, Hg.transl(0., length / 2., 0.))
shape = arboris.shapes.Box(f, half_extents)
world.register(shape)
return body
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)
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_table(world,
hl=(.05, .05, .05),
init_pos=eye(4),
mass=1.,
name='wall'):
""" Add a simple box to the world
"""
from arboris.massmatrix import box
from arboris.core import Body
from arboris.joints import TzJoint
## Create the body
M = box(hl, mass)
bbox = Body(mass=M, name=name)
world.register(Box(bbox, hl, name))
## Link the body to the world
world.add_link(SubFrame(world.ground, init_pos),
TzJoint(name=name + '_root'), bbox)
world.init()
def update(self, dt):
print "F.pose: ", self.f.pose
def finish(self):
pass
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
w.register(
Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.5), name='obstacle'),
radius=.05,
name='obstacle'))
w.register(
Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.63), name='obstacle'),
radius=.05,
name='obstacle2'))
#w.register(Cylinder(SubFrame(w.ground, transl(-.5, 0, 0.7), name='obstacle'),radius=.1, length=.2, name='obstacle') )
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
shapes = w.getshapes()
consts = w.getconstraints()
icub_joints = [joints[n] for n in icub.get_joints_data()]
from arboris.core import SubFrame
from arboris.shapes import Sphere, Cylinder
from arboris.homogeneousmatrix import transl
from arboris.constraints import SoftFingerContact
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_3r_and_init(gpos=(.5, .5, .5))
frames = w.getframes()
w.register(Sphere(frames['EndEffector'], radius=.02, name='ee'))
w.register(
Sphere(SubFrame(w.ground, transl(-.8, .2, 0.), name='obstacle'),
radius=.1,
name='obstacle'))
w.register(
Sphere(SubFrame(w.ground, transl(-.55, .2, 0.), name='obstacle2'),
radius=.1,
name='obstacle2'))
shapes = w.getshapes()
#w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01))
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
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()
legend(('simulated', 'theoric'))
w = World()
if True:
from arboris.homogeneousmatrix import transl
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
M = b.mass N = b.nleffects B = b.viscosity # information about its position in kinematic structure p_joint = b.parentjoint # return the parent joint c_joints = b.childrenjoints # return list of chidren joints ##### The SubFrame/MovingSubFrame classes ###################################### from arboris.core import SubFrame, MovingSubFrame from arboris.homogeneousmatrix import transl # How to create a fixed subframe H_b_f = transl(.1, .2, .3) # homogeneous matrix from body to subframe sf = SubFrame(body=w.ground, bpose=H_b_f, name="myFrame") # bpose constant # How to create a moving subframe #Warning: these are not designed to create the kinematic tree. # they are intended to be used as target for example msf = MovingSubFrame(w.ground, name="myMovingFrame") msf.bpose = H_b_f # possible only with MovingSubFrame instance ##### The Joint class ########################################################## # A joint relates a parent frame with a child frame by constraining their # relative motion. j = joints["Shoulder"] pose = j.pose # inverse method: ipose = rm.ipose() twist = j.twist # ..............: itwist
def add_link(body0, transl, joint, body1):
if not isinstance(body0, Body):
body0 = bodies[prefix + body0]
frame0 = SubFrame(body0, Hg.transl(*transl))
w.add_link(frame0, joint, bodies[prefix + body1])
