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