def create_inertia(self, inertia, color, name=""):
H_body_com = principalframe(inertia)
Mcom = transport(inertia, H_body_com)
mass = Mcom[5, 5]
m_o_inertia = Mcom[[0, 1, 2], [0, 1, 2]]
position = H_body_com[0:3, 3]
yaw, pitch, roll = np.degrees(
rotzyx_angles(H_body_com)) # in degrees for collada specification
self.physics_model.append(
E.rigid_body(E.technique_common(
E.dynamic("true"),
E.mass(str(mass)),
E.mass_frame(
E.translate(" ".join(map(str, position)), sid="location"),
E.rotate(" ".join(map(str, [0, 0, 1, yaw])),
sid="rotationZ"),
E.rotate(" ".join(map(str, [0, 1, 0, pitch])),
sid="rotationY"),
E.rotate(" ".join(map(str, [1, 0, 0, roll])),
sid="rotationX"),
),
E.inertia(" ".join(map(str, m_o_inertia))),
),
sid=name))
def zmp_position(bodies, g, gvel, dgvel, n=None):
"""
"""
R0_sum = zeros(3)
M0_sum = zeros(3)
for b in bodies:
Mbody = b.mass
m = Mbody[3, 3]
if m >= 1e-10:
H_body_com = principalframe(Mbody)
Mcom = transport(Mbody, H_body_com)
I = Mcom[0:3, 0:3]
P_com, J_com, dJ_com = body_com_properties(b)
twist = dot(J_com, gvel)
dtwist = dot(J_com, dgvel) + dot(dJ_com, gvel)
R0 = dot(m, (dtwist[3:6] - g))
M0 = dot(m, cross(P_com, (dtwist[3:6] - g))) + \
(dot(I, dtwist[0:3]) - cross(dot(I, twist[0:3]), twist[0:3]))
R0_sum += R0
M0_sum += M0
if n is None:
n = g/norm(g)
zmp = cross(n, M0_sum) / dot(R0_sum, n)
return zmp
def zmp_position(bodies, g, gvel, dgvel, n=None):
"""
"""
R0_sum = zeros(3)
M0_sum = zeros(3)
for b in bodies:
Mbody = b.mass
m = Mbody[3, 3]
if m >= 1e-10:
H_body_com = principalframe(Mbody)
Mcom = transport(Mbody, H_body_com)
I = Mcom[0:3, 0:3]
P_com, J_com, dJ_com = body_com_properties(b)
twist = dot(J_com, gvel)
dtwist = dot(J_com, dgvel) + dot(dJ_com, gvel)
R0 = dot(m, (dtwist[3:6] - g))
M0 = dot(m, cross(P_com, (dtwist[3:6] - g))) + \
(dot(I, dtwist[0:3]) - cross(dot(I, twist[0:3]), twist[0:3]))
R0_sum += R0
M0_sum += M0
if n is None:
n = g / norm(g)
zmp = cross(n, M0_sum) / dot(R0_sum, n)
return zmp
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_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 create_inertia(self, inertia, color, name=""):
"""Generate a representation of inertia as an ellipsoid."""
pydaenimColladaDriver.create_inertia(self, inertia, color)
H_body_com = principalframe(inertia)
Mcom = transport(inertia, H_body_com)
mass = Mcom[5,5]
m_o_inertia = Mcom[[0,1,2], [0,1,2]] * self._options['inertia factor']
node = _get_void_node()
node.transforms.append( collada.scene.MatrixTransform(H_body_com.flatten()) )
inertia_node = self.create_ellipsoid(m_o_inertia, color, name+".inertia")
node.children.append(inertia_node)
for c in inertia_node.children:
if isinstance(c, collada.scene.ExtraNode):
inertia_node.children.remove(c)
self._add_osg_description(inertia_node, "inertia")
return node
def add_robot(w, gpos, gvel, is_fixed=True):
assert isinstance(w, World)
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.
M = transport(cylinder(length, radius, mass),
transl(0., -length/2., 0.))
body = Body(mass=M)
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 create_inertia(self, inertia, color, name=""):
"""Generate a representation of inertia as an ellipsoid."""
pydaenimColladaDriver.create_inertia(self, inertia, color)
H_body_com = principalframe(inertia)
Mcom = transport(inertia, H_body_com)
mass = Mcom[5, 5]
m_o_inertia = Mcom[[0, 1, 2],
[0, 1, 2]] * self._options['inertia factor']
node = _get_void_node()
node.transforms.append(
collada.scene.MatrixTransform(H_body_com.flatten()))
inertia_node = self.create_ellipsoid(m_o_inertia, color,
name + ".inertia")
node.children.append(inertia_node)
for c in inertia_node.children:
if isinstance(c, collada.scene.ExtraNode):
inertia_node.children.remove(c)
self._add_osg_description(inertia_node, "inertia")
return node
def create_inertia(self, inertia, color, name=""):
H_body_com = principalframe(inertia)
Mcom = transport(inertia, H_body_com)
mass = Mcom[5, 5]
m_o_inertia = Mcom[[0, 1, 2], [0, 1, 2]]
position = H_body_com[0:3, 3]
yaw, pitch, roll = np.degrees(rotzyx_angles(H_body_com)) # in degrees for collada specification
self.physics_model.append(
E.rigid_body(
E.technique_common(
E.dynamic("true"),
E.mass(str(mass)),
E.mass_frame(
E.translate(" ".join(map(str, position)), sid="location"),
E.rotate(" ".join(map(str, [0, 0, 1, yaw])), sid="rotationZ"),
E.rotate(" ".join(map(str, [0, 1, 0, pitch])), sid="rotationY"),
E.rotate(" ".join(map(str, [1, 0, 0, roll])), sid="rotationX"),
),
E.inertia(" ".join(map(str, m_o_inertia))),
),
sid=name,
)
)
plot(self.timeline, self.height,
self.timeline, self.get_theoric())
legend(('simulated', 'theoric'))
w = ObservableWorld()
if True:
from arboris.homogeneousmatrix import transl
H_bc = transl(1,1,1)
else:
H_bc = eye(4)
lengths = (1.,1.,1.)
mass = 1.
body = Body(
name='box_body',
mass=massmatrix.transport(massmatrix.box(lengths, 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, lengths))
weightc = WeightController(w)
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
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)
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)
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}
print("H_com_a * H_a_com\n", np.dot(H_com_a, H_a_com))
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()
tauLim = icub.get_torque_limits()
# interesting values we want to obtain
H_body_com = {}
body_mass = {}
body_moment_of_inertia = {}
H_parentCoM_bCoM = {}
list_of_dof = {}
list_of_children = {}
# We compute the displacement from the body frame to the body center of mass, aligned with the principal axis of inertia
H_body_com["ground"] = eye(4)
for b in w.getbodies()[1:]:
H_body_com[b.name] = principalframe(b.mass)
Mb_com = transport(b.mass, H_body_com[b.name]) # if all good, Mb_com is diagonal
body_mass[b.name] = Mb_com[5,5]
body_moment_of_inertia[b.name] = tuple(Mb_com[[0,1,2], [0,1,2]])
# We compute the displacement between the centers of mass:
# H_parentCoM_bCoM = H_parentCoM_parent * H_parent_b * H_b_bCoM
# <=> (H_0_parent * H_parent_parentCoM)^(-1) * (H_0_b * H_b_bCoM)
for b in w.getbodies()[1:]:
parent = b.parentjoint.frame0.body
H_parentCoM_bCoM[b.name] = dot( Hinv( dot(parent.pose, H_body_com[parent.name]) ), dot(b.pose, H_body_com[b.name]) )
