def testSubFrames(self):
body = Body()
body.update_dynamic(self.alea_pose, self.alea_jac, self.alea_djac,
self.alea_twist)
sframe = SubFrame(body, Hg.rotz(3.14/3.), 'Brand New Frame')
self.assertListsAlmostEqual(sframe.bpose,
array([ [ 0.50045969, -0.86575984, 0. , 0. ],
[ 0.86575984, 0.50045969, 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]]))
self.assertListsAlmostEqual(sframe.pose,
[ [ 4.3287992 , 2.50229845, 0., 0. ],
[ 2.16695503, -1.75051589, 0., 4.2 ],
[ 0. , 0. , 0., 0. ],
[ 7.79183856, 4.5041372 , 0., 3. ] ])
self.assertListsAlmostEqual(sframe.jacobian,
[ [ 2.10193069, 4.00367751, 0.50045969],
[-3.63619133, -6.92607872, -0.86575984],
[ 0. , 7. , 0. ],
[ 1.30119519, 0. , 0.43287992],
[-2.25097558, 0. , 0.25022984],
[ 8.8 , 0. , 2. ] ])
self.assertListsAlmostEqual(sframe.djacobian,
[ [ 0.50045969, 36.76228101, 0. ],
[ -0.86575984, 20.32669907, 0. ],
[ 0. , 4.2 , 2.6 ],
[ 0.50045969, 0.86575984, 0. ],
[ -0.86575984, 0.50045969, 0. ],
[ 7.1 , 6. , 0. ] ])
self.assertListsAlmostEqual(sframe.twist,
[ 0., 0., 7.1, 36.86237295, 20.1535471, 0. ])
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 convert_nonroot_joint(self, world, joint):
"""Convert a joint, which is assume not to be the root one of the robot."""
assert isinstance(joint, Joint)
assert isinstance(world, World)
assert joint.frames[0].body is not world.ground
frame0, frame1 = joint.frames
def name(suffix):
if joint.name is None:
return None
else:
return joint.name + suffix
def mass():
from numpy import diag
return self.base_mass * diag([1.0, 1.0, 1.0, 0.1, 0.1, 0.1])
if isinstance(joint, joints.RzJoint) or \
isinstance(joint, joints.RyJoint) or \
isinstance(joint, joints.RxJoint):
# nothing to do, the joint is compatible
pass
elif isinstance(joint, joints.RzRyRxJoint):
Rz = joints.RzJoint(name=name('Rz'))
Bzy = Body(mass=mass())
Ry = joints.RyJoint(name=name('Ry'))
Byx = Body(mass=mass())
Rx = joints.RxJoint(name=name('Rx'))
world.replace_joint(joint, frame0, Rz, Bzy, Bzy, Ry, Byx, Byx, Rx,
frame1)
elif isinstance(joint, joints.RzRyJoint):
Rz = joints.RzJoint(name=name('Rz'))
Bzy = Body(mass=mass())
Ry = joints.RyJoint(name=name('Ry'))
world.replace_joint(joint, frame0, Rz, Bzy, Bzy, Ry, frame1)
elif isinstance(joint, joints.RyRxJoint):
Ry = joints.RyJoint(name=name('Ry'))
Byx = Body(mass=mass())
Rx = joints.RxJoint(name=name('Rx'))
world.replace_joint(joint, frame0, Ry, Byx, Byx, Rx, frame1)
elif isinstance(joint, joints.RzRxJoint):
Rz = joints.RzJoint(name=name('Rz'))
Bzx = Body(mass=mass())
Rx = joints.RxJoint(name=name('Rx'))
world.replace_joint(joint, frame0, Rz, Bzx, Bzx, Rx, frame1)
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 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))
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 setUp(self):
"""
Ensure multi-dof Rzyx behaves like the combination of Rz, Ry, Rx.
"""
world = World()
self.Ba = Body()
Rzyx = RzRyRxJoint()
world.add_link(world.ground, Rzyx, self.Ba)
Bzy = Body(name='zy')
Byx = Body(name='yx')
self.Bb = Body()
Rz = RzJoint()
world.add_link(world.ground, Rz, Bzy)
Ry = RyJoint()
world.add_link(Bzy, Ry, Byx)
Rx = RxJoint()
world.add_link(Byx, Rx, self.Bb)
world.init()
(az, ay, ax) = (3.14/6, 3.14/4, 3.14/3)
Rzyx.gpos[:] = (az, ay, ax)
(Rz.gpos[0], Ry.gpos[0], Rx.gpos[0]) = (az, ay, ax)
world.update_dynamic()
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 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 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()
def plot_height(self):
plot(self.timeline, self.height, self.timeline, self.get_theoric())
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)
pose = f.pose # return H_0_f (0 is world.ground) bpose = f.bpose # return H_b_f (b is f.body) twist = f.twist # return T_f_0_f (T of f relative to 0 expressed in f) jacobian = f.jacobian # return J_f_0_f such as T_f_0_f = J_f_0_f * gvel djacobian = f.djacobian # return dJ_f_0_f ##### The Body class ########################################################### # It derives from Frame class, so properties above are available. # They are rigid and represent "nodes" in the kinematic tree. # How to create a body from arboris.core import Body from arboris.massmatrix import sphere M_sphere = sphere(radius=.1, mass=1.) b = Body(mass=M_sphere, viscosity=None, name="myBody") # body properties 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
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()
