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]) )