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 init(self, world, timeline):
self._world = world
self.time = []
self.kinetic_energy = []
self.potential_energy = []
self.mechanichal_energy = []
self._com_pos = {}
self._bodies = self._world.ground.iter_descendant_bodies
for body in self._bodies():
self._com_pos[body] = principalframe(body.mass)[:,3]
def init(self, world, timeline=None):
self.H_body_com = []
self.mass = []
self.bodies = [b for b in self.user_bodies if b.mass[5, 5] > 0]
for b in self.bodies:
self.H_body_com.append(principalframe(b.mass))
self.mass.append(b.mass[5, 5])
self.total_mass = sum(self.mass)
self._CoMPosition = zeros(3)
self._CoMJacobian = zeros((3, world.ndof))
self._CoMdJacobian = zeros((3, world.ndof))
def update(self, dt=None):
gforce = zeros(self._wndof)
for b in self._bodies():
#TODO: improve efficiency
from arboris.massmatrix import principalframe
H_bc = principalframe(b.mass)
R_gb = b.pose[0:3,0:3]
H_bc[0:3,0:3] = R_gb.T
#wrench_c = array((0,0,0,0,-9.81*b.mass[3,3],0))
#Ad_cb = homogeneousmatrix.iadjoint(H_bc)
#wrench_b = dot(Ad_cb.T, wrench_c)
#gforce += dot(b.jacobian.T, wrench_b )
# gravity acceleration expressed in body frame
g = dot(homogeneousmatrix.iadjoint(b.pose), self._gravity_dtwist)
gforce += dot(b.jacobian.T, dot(b.mass, g))
impedance = zeros( (self._wndof, self._wndof) )
return (gforce, impedance)
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 body_com_properties(body, compute_J=True):
""" Compute the Center of Mass properties of a body.
"""
H_body_com = principalframe(body.mass)
H_0_com = dot(body.pose, H_body_com)
P_0_com = H_0_com[0:3, 3]
if compute_J:
H_com_com2 = inv(H_0_com)
H_com_com2[0:3, 3] = 0.
Ad_com2_body = iadjoint(dot(H_body_com, H_com_com2))
J_com2 = dot(Ad_com2_body, body.jacobian)
T_com2_body = body.twist.copy()
T_com2_body[3:6] = 0.
dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body)
dJ_com2 = dot(Ad_com2_body, body.djacobian) + \
dot(dAd_com2_body, body.jacobian)
return P_0_com, J_com2, dJ_com2
else:
return P_0_com
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,
)
)
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))
jLim = icub.get_joint_limits(in_radian=True)
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]) )
