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 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()
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()]
standing_pose = [j.gpos[0] for j in icub_joints]
from common import create_3r_and_init, get_usual_observers
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))
#########################################
# #
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()
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")
##### The Constraint class #####################################################
# A constraint are actually a mean to create a loop in the kinematic structure.
# It generates a force to ensure that the loop is closed.
# how instanciate some contraint class
from arboris.constraints import JointLimits, BallAndSocketConstraint, SoftFingerContact
j = joints["Shoulder"]
c = JointLimits(joint=j, min_limits=[-3.14], max_limits=[3.14], name="JLim")
f0, f1 = w.ground, frames["EndEffector"]