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_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(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()
body = f.body # return parent body (itself if f is body) 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
##### About twist ##############################################################
rot_velocity = [.1,.2,.3]
lin_velocity = [.4,.5,.6]
twist = np.array(rot_velocity + lin_velocity) # rotation velocity first
print("twist", twist)
##### About mass matrix ########################################################
import arboris.massmatrix as Mm
# mass matrix expressed in principal inertia frame, length in "m", mass in "kg"
M_com = Mm.sphere(radius=.1, mass=10)
M_com = Mm.ellipsoid(radii=(.1, .2, .3), mass=10)
M_com = Mm.cylinder(length=.1, radius=.2, mass=10) # revolution axis along z
M_com = Mm.box(half_extents=(.1, .2, .3), mass=10)
isMm = Mm.ismassmatrix(M_com) # check validity of mass matrix
print("is mass matrix?", isMm)
H_com_a= Hg.transl(.4,.5,.6) # translation from {a} (new frame) to {com}
M_a = Mm.transport(M_com, H_com_a)
H_a_com = Mm.principalframe(M_a) # return principal inertia frame from {a}
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()
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 ######################################
