def add_simplearm(world, name='', lengths=(0.5, 0.4, 0.2), masses=(1.0, 0.8, 0.2), with_shapes=False): """Build a planar 3-R robot. ***Example:*** >>> w = World() >>> add_simplearm(w) >>> w.update_dynamic() """ assert isinstance(world, World) 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 # create the 3 bodies arm = create_body(name + 'Arm', lengths[0], masses[0]) forearm = create_body(name + 'Forearm', lengths[1], masses[1]) hand = create_body(name + 'Hand', lengths[2], masses[2]) # create a joint between the ground and the arm shoulder = RzJoint(name=name + 'Shoulder') world.add_link(world.ground, shoulder, arm) # add a frame to the arm, where the forearm will be anchored f = SubFrame(arm, Hg.transl(0, lengths[0], 0), name + 'ElbowBaseFrame') # create a joint between the arm and the forearm elbow = RzJoint(name=name + 'Elbow') world.add_link(f, elbow, forearm) # add a frame to the forearm, where the hand will be anchored f = SubFrame(forearm, Hg.transl(0, lengths[1], 0), name + 'WristBaseFrame') # create a joint between the forearm and the hand wrist = RzJoint(name=name + 'Wrist') world.add_link(f, wrist, hand) # create a frame at the end of the hand f = SubFrame(hand, Hg.transl(0, lengths[2], 0), name + 'EndEffector') world.register(f) world.init()
def create_icub_and_init(chair=False, gravity=False): ## CREATE THE WORLD w = World() w._up[:] = [0, 0, 1] icub.add(w, create_shapes=False) w.register(Plane(w.ground, (0, 0, 1, 0), "floor")) if chair is True: w.register(Sphere(w.getbodies()['l_hip_2'], .0325, name='l_gluteal')) w.register(Sphere(w.getbodies()['r_hip_2'], .0325, name='r_gluteal')) w.register( Box(SubFrame(w.ground, transl(.2, 0, 0.26)), (.075, .1, .02), name='chair')) w.register( Box(SubFrame(w.ground, transl(.255, 0, 0.36)), (.02, .1, .1), name='chair_back')) ## INIT joints = w.getjoints() joints['root'].gpos[0:3, 3] = [0, 0, .598] joints['l_shoulder_roll'].gpos[:] = pi / 8 joints['r_shoulder_roll'].gpos[:] = pi / 8 joints['l_elbow_pitch'].gpos[:] = pi / 8 joints['r_elbow_pitch'].gpos[:] = pi / 8 joints['l_knee'].gpos[:] = -0.1 joints['r_knee'].gpos[:] = -0.1 joints['l_ankle_pitch'].gpos[:] = -0.1 joints['r_ankle_pitch'].gpos[:] = -0.1 shapes = w.getshapes() floor_const = [ SoftFingerContact((shapes[s], shapes['floor']), 1.5, name=s) for s in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4'] ] for c in floor_const: w.register(c) if chair is True: chair_const = [ SoftFingerContact((shapes[s], shapes['chair']), 1.5, name=s) for s in ['l_gluteal', 'r_gluteal'] ] for c in chair_const: w.register(c) w.update_dynamic() ## CTRL if gravity: w.register(WeightController()) return w
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 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 add_tag(name, body, position): """Returns data about anatomical landmarks as defined in HuMAnS. :param name: name of the tag (copied from HuMAnS) :param body: name of the body in whose frame the point is defined :position: tag coordinates in meters """ if name: name = prefix + name tag = SubFrame(bodies[prefix + body], Hg.transl(*position), name) tags.append(tag) w.register(tag)
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_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 update(self, dt): print "F.pose: ", self.f.pose def finish(self): pass ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_icub_and_init(gravity=True) w.register( Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.5), name='obstacle'), radius=.05, name='obstacle')) w.register( Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.63), name='obstacle'), radius=.05, name='obstacle2')) #w.register(Cylinder(SubFrame(w.ground, transl(-.5, 0, 0.7), name='obstacle'),radius=.1, length=.2, name='obstacle') ) joints = w.getjoints() frames = w.getframes() bodies = w.getbodies() shapes = w.getshapes() consts = w.getconstraints() icub_joints = [joints[n] for n in icub.get_joints_data()]
from arboris.core import SubFrame from arboris.shapes import Sphere, Cylinder from arboris.homogeneousmatrix import transl from arboris.constraints import SoftFingerContact ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(.5, .5, .5)) frames = w.getframes() w.register(Sphere(frames['EndEffector'], radius=.02, name='ee')) w.register( Sphere(SubFrame(w.ground, transl(-.8, .2, 0.), name='obstacle'), radius=.1, name='obstacle')) w.register( Sphere(SubFrame(w.ground, transl(-.55, .2, 0.), name='obstacle2'), radius=.1, name='obstacle2')) shapes = w.getshapes() #w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01)) ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # #
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()
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) from arboris.visu_osg import Drawer
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 # How to create a fixed subframe H_b_f = transl(.1, .2, .3) # homogeneous matrix from body to subframe sf = SubFrame(body=w.ground, bpose=H_b_f, name="myFrame") # bpose constant # How to create a moving subframe #Warning: these are not designed to create the kinematic tree. # they are intended to be used as target for example msf = MovingSubFrame(w.ground, name="myMovingFrame") msf.bpose = H_b_f # possible only with MovingSubFrame instance ##### The Joint class ########################################################## # A joint relates a parent frame with a child frame by constraining their # relative motion. j = joints["Shoulder"] pose = j.pose # inverse method: ipose = rm.ipose() twist = j.twist # ..............: itwist
def add_link(body0, transl, joint, body1): if not isinstance(body0, Body): body0 = bodies[prefix + body0] frame0 = SubFrame(body0, Hg.transl(*transl)) w.add_link(frame0, joint, bodies[prefix + body1])