def testSubFrames(self): body = Body() body.update_dynamic(self.alea_pose, self.alea_jac, self.alea_djac, self.alea_twist) sframe = SubFrame(body, Hg.rotz(3.14/3.), 'Brand New Frame') self.assertListsAlmostEqual(sframe.bpose, array([ [ 0.50045969, -0.86575984, 0. , 0. ], [ 0.86575984, 0.50045969, 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]])) self.assertListsAlmostEqual(sframe.pose, [ [ 4.3287992 , 2.50229845, 0., 0. ], [ 2.16695503, -1.75051589, 0., 4.2 ], [ 0. , 0. , 0., 0. ], [ 7.79183856, 4.5041372 , 0., 3. ] ]) self.assertListsAlmostEqual(sframe.jacobian, [ [ 2.10193069, 4.00367751, 0.50045969], [-3.63619133, -6.92607872, -0.86575984], [ 0. , 7. , 0. ], [ 1.30119519, 0. , 0.43287992], [-2.25097558, 0. , 0.25022984], [ 8.8 , 0. , 2. ] ]) self.assertListsAlmostEqual(sframe.djacobian, [ [ 0.50045969, 36.76228101, 0. ], [ -0.86575984, 20.32669907, 0. ], [ 0. , 4.2 , 2.6 ], [ 0.50045969, 0.86575984, 0. ], [ -0.86575984, 0.50045969, 0. ], [ 7.1 , 6. , 0. ] ]) self.assertListsAlmostEqual(sframe.twist, [ 0., 0., 7.1, 36.86237295, 20.1535471, 0. ])
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 convert_nonroot_joint(self, world, joint): """Convert a joint, which is assume not to be the root one of the robot.""" assert isinstance(joint, Joint) assert isinstance(world, World) assert joint.frames[0].body is not world.ground frame0, frame1 = joint.frames def name(suffix): if joint.name is None: return None else: return joint.name + suffix def mass(): from numpy import diag return self.base_mass * diag([1.0, 1.0, 1.0, 0.1, 0.1, 0.1]) if isinstance(joint, joints.RzJoint) or \ isinstance(joint, joints.RyJoint) or \ isinstance(joint, joints.RxJoint): # nothing to do, the joint is compatible pass elif isinstance(joint, joints.RzRyRxJoint): Rz = joints.RzJoint(name=name('Rz')) Bzy = Body(mass=mass()) Ry = joints.RyJoint(name=name('Ry')) Byx = Body(mass=mass()) Rx = joints.RxJoint(name=name('Rx')) world.replace_joint(joint, frame0, Rz, Bzy, Bzy, Ry, Byx, Byx, Rx, frame1) elif isinstance(joint, joints.RzRyJoint): Rz = joints.RzJoint(name=name('Rz')) Bzy = Body(mass=mass()) Ry = joints.RyJoint(name=name('Ry')) world.replace_joint(joint, frame0, Rz, Bzy, Bzy, Ry, frame1) elif isinstance(joint, joints.RyRxJoint): Ry = joints.RyJoint(name=name('Ry')) Byx = Body(mass=mass()) Rx = joints.RxJoint(name=name('Rx')) world.replace_joint(joint, frame0, Ry, Byx, Byx, Rx, frame1) elif isinstance(joint, joints.RzRxJoint): Rz = joints.RzJoint(name=name('Rz')) Bzx = Body(mass=mass()) Rx = joints.RxJoint(name=name('Rx')) world.replace_joint(joint, frame0, Rz, Bzx, Bzx, Rx, frame1)
def convert_root_joint(self, world, root_joint): assert isinstance(root_joint, Joint) assert isinstance(world, World) assert root_joint.frames[0].body is world.ground if isinstance(root_joint, joints.FreeJoint): # nothing to do pass else: from arboris.homogeneousmatrix import transl from arboris.massmatrix import box as boxmass from arboris.shapes import Box, Sphere from arboris.constraints import SoftFingerContact # add a "free-floating" base body to the robot base = Body(mass=boxmass(self.base_lengths, self.base_mass)) world.replace_joint(root_joint, root_joint.frames[0], joints.FreeJoint(), base, base, root_joint, root_joint.frames[1]) # add a ground plane r = self.contact_radius ground_half_extents = [d / 2. + 2 * r for d in self.base_lengths] ground_plane = Box(world.ground, ground_half_extents) world.register(ground_plane) # put 4 spheres at the bottom of the base (x, y, z) = self.base_lengths for (i, j) in ((1, 1), (1, -1), (-1, -1), (-1, 1)): sf = SubFrame(base, transl(i * x / 2, -y / 2, j * z / 2)) sh = Sphere(sf, r) world.register(sh) contact = SoftFingerContact((ground_plane, sh), self.friction_coeff) world.register(contact)
def add_body(name, mass, com_position, gyration_radius): #mass matrix at com mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1)))) H_fg = eye(4) H_fg[0:3, 3] = com_position H_gf = Hg.inv(H_fg) #mass matrix at body's frame origin: mass_o = dot(Hg.adjoint(H_gf).T, dot(mass_g, Hg.adjoint(H_gf))) bodies.append(Body(name=prefix + name, mass=mass_o))
def create_body(name, length, mass): """create a body""" half_extents = (length / 20., length / 2., length / 20.) mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass) mass_matrix_at_base = arboris.massmatrix.transport( mass_matrix_at_com, Hg.transl(0., -length / 2., 0.)) body = Body(name, mass_matrix_at_base) if with_shapes: f = SubFrame(body, Hg.transl(0., length / 2., 0.)) shape = arboris.shapes.Box(f, half_extents) world.register(shape) return body
def setUp(self): """ Ensure multi-dof Rzyx behaves like the combination of Rz, Ry, Rx. """ world = World() self.Ba = Body() Rzyx = RzRyRxJoint() world.add_link(world.ground, Rzyx, self.Ba) Bzy = Body(name='zy') Byx = Body(name='yx') self.Bb = Body() Rz = RzJoint() world.add_link(world.ground, Rz, Bzy) Ry = RyJoint() world.add_link(Bzy, Ry, Byx) Rx = RxJoint() world.add_link(Byx, Rx, self.Bb) world.init() (az, ay, ax) = (3.14/6, 3.14/4, 3.14/3) Rzyx.gpos[:] = (az, ay, ax) (Rz.gpos[0], Ry.gpos[0], Rx.gpos[0]) = (az, ay, ax) world.update_dynamic()
def add_cylinder(world, length=1., radius=1., mass=1., name='Cylinder'): """Build a cylinder robot, whose symmetry axis is along the z-axis. Example: >>> w = World() >>> add_cylinder(w) >>> w.update_dynamic() """ assert isinstance(world, World) cylinder = Body(name=name, mass=massmatrix.cylinder(length, radius, mass)) world.add_link(world.ground, FreeJoint(), cylinder) world.register(Cylinder(cylinder, length, radius)) world.init()
def add_box(world, half_extents=(1., 1., 1.), mass=1., name='Box'): """Build a box robot. Example: >>> w = World() >>> add_box(w) >>> w.update_dynamic() """ assert isinstance(world, World) box = Body(name=name, mass=massmatrix.box(half_extents, mass)) world.add_link(world.ground, FreeJoint(), box) world.register(Box(box, half_extents)) 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_table(world, hl=(.05, .05, .05), init_pos=eye(4), mass=1., name='wall'): """ Add a simple box to the world """ from arboris.massmatrix import box from arboris.core import Body from arboris.joints import TzJoint ## Create the body M = box(hl, mass) bbox = Body(mass=M, name=name) world.register(Box(bbox, hl, name)) ## Link the body to the world world.add_link(SubFrame(world.ground, init_pos), TzJoint(name=name + '_root'), bbox) world.init()
def runTest(self): b0 = Body(mass=eye(6)) w = World() w.add_link(w.ground, FreeJoint(), b0) w.init() ctrl = WeightController() w.register(ctrl) c0 = BallAndSocketConstraint(frames=(w.ground, b0)) w.register(c0) w.init() w.update_dynamic() dt = 0.001 w.update_controllers(dt) w.update_constraints(dt) self.assertListsAlmostEqual(c0._force, [0., 9.81, 0.]) w.integrate(dt) w.update_dynamic() self.assertListsAlmostEqual( b0.pose, [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
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)
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
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()