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 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 get_joints_data(): """ This dictionnary describes the joint of the robot Each joint has 3 arguments name : [parent_body, H_parentbody_jointframe, child_body] parent_body: the name parent body (a string) H_parentbody_jointframe: give the transformation matrix from the parent_body frame to the joint frame. The joint is always a RzJoint, a rotation around z axis. chid_body: the child body frame (a string) which is linked with the joint to the parent body. """ return { 'torso_pitch': ['waist' , dot(rotz(pi/2), roty(-pi/2)), 'lap_belt_1'], 'torso_roll': ['lap_belt_1', T_DH(0, 0, .032, pi/2) , 'lap_belt_2'], 'torso_yaw': ['lap_belt_2', T_DH(0, -pi/2, 0, pi/2) , 'chest'], 'head_pitch': ['chest' , T_DH(-.1933, -pi/2, .00231, -pi/2), 'neck_1'], 'head_roll': ['neck_1', T_DH(0, pi/2, .033, pi/2) , 'neck_2'], 'head_yaw': ['neck_2', T_DH(.001, -pi/2, 0, -pi/2) , 'head'], 'l_shoulder_pitch': ['chest' , T_DH(-.1433, 105*pi/180, .0233647,-pi/2), 'l_shoulder_1'], 'l_shoulder_roll': ['l_shoulder_1', T_DH(.10774, pi/2, 0, -pi/2) , 'l_shoulder_2'], 'l_shoulder_yaw': ['l_shoulder_2', T_DH(0, -pi/2, 0, pi/2) , 'l_arm'], 'l_elbow_pitch': ['l_arm' , T_DH(.15228, 75*pi/180, 0, -pi/2) , 'l_elbow_1'], 'l_elbow_yaw': ['l_elbow_1' , T_DH(0, 0, -.015, pi/2) , 'l_forearm'], 'l_wrist_roll': ['l_forearm' , T_DH(.1373, -pi/2, 0, pi/2) , 'l_wrist_1'], 'l_wrist_pitch': ['l_wrist_1' , T_DH(0, pi/2, 0, pi/2) , 'l_hand'], 'r_shoulder_pitch': ['chest' , T_DH(-.1433,-105*pi/180,-.0233647, pi/2), 'r_shoulder_1'], 'r_shoulder_roll': ['r_shoulder_1', T_DH(-.10774, -pi/2, 0, pi/2) , 'r_shoulder_2'], 'r_shoulder_yaw': ['r_shoulder_2', T_DH(0, -pi/2, 0, -pi/2) , 'r_arm'], 'r_elbow_pitch': ['r_arm' , T_DH(-.15228, -105*pi/180, 0, -pi/2) , 'r_elbow_1'], 'r_elbow_yaw': ['r_elbow_1' , T_DH(0, 0, .015, pi/2) , 'r_forearm'], 'r_wrist_roll': ['r_forearm' , T_DH(-.1373, -pi/2, 0, pi/2) , 'r_wrist_1'], 'r_wrist_pitch': ['r_wrist_1' , T_DH(0, pi/2, 0, pi/2) , 'r_hand'], 'l_hip_pitch': ['waist' , dot(transl(0,-.0681, -.1199), rotx(-pi/2)), 'l_hip_1'], 'l_hip_roll': ['l_hip_1' , T_DH(0, pi/2, 0, -pi/2) , 'l_hip_2'], 'l_hip_yaw': ['l_hip_2' , T_DH(0, pi/2, 0, -pi/2) , 'l_thigh'], 'l_knee': ['l_thigh' , T_DH(-.2236 , -pi/2, 0, pi/2) , 'l_shank'], 'l_ankle_pitch': ['l_shank' , T_DH(0 , pi/2, -.213 , pi) , 'l_ankle_1'], 'l_ankle_roll': ['l_ankle_1', T_DH(0, 0, 0, -pi/2) , 'l_foot'], 'r_hip_pitch': ['waist' , dot(transl(0, .0681, -.1199), rotx(-pi/2)), 'r_hip_1'], 'r_hip_roll': ['r_hip_1' , T_DH(0, pi/2, 0, pi/2) , 'r_hip_2'], 'r_hip_yaw': ['r_hip_2' , T_DH(0, pi/2, 0, pi/2) , 'r_thigh'], 'r_knee': ['r_thigh' , T_DH(.2236 , -pi/2, 0, -pi/2) , 'r_shank'], 'r_ankle_pitch': ['r_shank' , T_DH(0 , pi/2, -.213 , pi) , 'r_ankle_1'], 'r_ankle_roll': ['r_ankle_1', T_DH(0, 0, 0, pi/2) , 'r_foot'], }
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 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 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 get_bodies_shapes_data(): """ """ bodies_shapes_data = {} for name, data in get_bodies_data().items(): bodies_shapes_data[name] = [[d[0], d[2]] for d in data] bodies_shapes_data["head"].extend([ [(.02, ), transl(-.034, -.054, .0825)], #l_eye [(.02, ), transl(.034, -.054, .0825)], #r_eye ]) return bodies_shapes_data
def get_bodies_shapes_data(): """ """ bodies_shapes_data = {} for name, data in get_bodies_data().items(): bodies_shapes_data[name] = [[d[0], d[2]] for d in data] bodies_shapes_data["head"].extend([ [(.02,), transl(-.034, -.054, .0825)], #l_eye [(.02,), transl( .034, -.054, .0825)], #r_eye ]) return bodies_shapes_data
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_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 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_lengths = [ d+4*r for d in self.base_lengths] ground_plane = Box(world.ground, ground_lengths) 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_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_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 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_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_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 testLinksCreation(self): w = World() bulb = RyRxJoint(name='bulb') stem = Body(name='stem', mass=eye(6)*0.1) w.add_link(w.ground, bulb, stem) stem_top = SubFrame(stem, bpose=transl(0, 0, 1), name='stem_top') flowerR = RyJoint(gpos=.5, name='flowerR') leafR = Body('leafR', mass=eye(6)*0.01) w.add_link(stem_top, flowerR, leafR) flowerL = RyJoint(gpos=(-.5), name='flowerL') leafL = Body('leafL', mass=eye(6)*.01) w.add_link(stem_top, flowerL, leafL) frames = [ w.ground, stem, leafR, leafL, stem_top ] for frame1, frame2 in zip(w.iterframes(), frames): self.assertEqual(frame1, frame2) joints = [ bulb, flowerR, flowerL ] for joint1, joint2 in zip(w.iterjoints(), joints): self.assertEqual(joint1, joint2)
""" __author__ = ("Joseph Salini <*****@*****.**>") from arboris.core import ObservableWorld, simulate, SubFrame from arboris.homogeneousmatrix import transl from arboris.robots.simplearm import add_simplearm from arboris.visu_osg import Drawer from arboris.qpcontroller import BalanceController, Task #from arboris.controllers import WeightController from numpy import arange world = ObservableWorld() #world.observers.append(Drawer(world)) add_simplearm(world, name='Left') world.register(SubFrame(world.ground, transl(0.5, 0.5, 0), 'LeftTarget')) joints = world.getjoints() frames = world.getframes() # set initial position: joints[0].gpos = [0.1] task = Task(controlled_frame=frames['LeftEndEffector'], target_frame=frames['LeftTarget'], world=world) #world.register(WeightController(world)) world.register(BalanceController(world, [task])) from arboris.core import JointsList add_simplearm(world, name='Right') joints=world.getjoints()
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.
def get_bodies_data(): """ This dictionnary describes the bodies of the robot Each body is a list of shape name : [ [(dims1) | mass1 | H1], [(dims2) | mass2 | H2] ] about dims (in meter): if dims has 1 element: it is a sphere; get (radius,) if dims has 2 elements: it is a cylinder with z-axis; get (length, radius) if dims has 3 elements: it is a box; get half-lengths along (x,y,z) masses are in kg about transformation matrix H: it represents the transformation from the body frame to the center of the shape. transl for translation along (x,y,z) (meter) rotx,roty, rotz for rotation around x,y,z axis dot for matrices multiplication """ return { ### WARNING: pb between shape and inertia in the c++ file of the simulator #'waist' : [[(0.002, 0.065, 0.027), 0.20297, dot(transl(0, 0, -0.065), rotx(pi/2))]], 'waist' : [[(.032, .0235, .057215), .20297, transl(.006, 0,-.1)]], 'lap_belt_1': [[(.0315, .0635, .088) , 3.623 , dot(transl(-.0383, -.0173, 0), rotz(-pi/4))]], 'lap_belt_2': [[(.097, .031) , .91179, transl(0, 0, .008)]], 'chest': [[(.0274, .04) , .45165, transl(0, 0, -.0447)], [(.036, .0545, .059) , 1.8388, dot(transl(-.03, 0,-.1174), rotz( pi/12))], [(.036, .0545, .059) , 1.8388, dot(transl( .03, 0,-.1174), rotz(-pi/12))]], 'neck_1': [[(.077, .015), .28252, eye(4)]], 'neck_2': [[(.033, .015), .1 , eye(4)]], 'head': [[(.08,) , .78 , transl(0, 0, .0825)]], 'l_shoulder_1': [[(.011, .031) , .48278 , transl(0, 0, .07224)]], 'l_shoulder_2': [[(.059, .03) , .20779 , eye(4)]], 'l_arm': [[(.156, .026) , 1.1584 , transl(0, 0, .078)]], 'l_elbow_1': [[(.01,) , .050798, eye(4)]], 'l_forearm': [[(.14, .02) , .48774 , transl(0, 0, .07)]], 'l_wrist_1': [[(.04, .01) , .05 , eye(4)]], 'l_hand': [[(.0345, .0325, .012), .19099 , transl(.0345, 0, 0)]], 'r_shoulder_1': [[(.011, .031) , .48278 , transl(0, 0, -.07224)]], 'r_shoulder_2': [[(.059, .03) , .20779 , eye(4)]], 'r_arm': [[(.156, .026) , 1.1584 , transl(0, 0, -.078)]], 'r_elbow_1': [[(.01,) , .050798, eye(4)]], 'r_forearm': [[(.14, .02) , .48774 , transl(0, 0, -.07)]], 'r_wrist_1': [[(.04, .01) , .05 , eye(4)]], 'r_hand': [[(.0345, .0325, .012), .19099 , transl(-.0345, 0, 0)]], 'l_hip_1': [[(.013, .038) , .32708, transl(0, 0, .0375)]], 'l_hip_2': [[(.075, .031) , 1.5304, eye(4)]], 'l_thigh': [[(.224, .034) , 1.5304, transl(0, 0, -.112)], [(.077,.0315) , .79206, dot(transl(0, 0, -.224), roty(pi/2))]], 'l_shank': [[(.213, .0315) , .95262, dot(transl(0, -.1065, 0), rotx(pi/2))]], 'l_ankle_1': [[(.063, .0245) , .14801, eye(4)]], 'l_foot': [[(.095, .027) , .59285, transl(0,0,.0125)], [(.002, .027, .065), .08185, transl(-.039, 0, .034)]], 'r_hip_1': [[(.013, .038) , .32708, transl(0, 0, -.0375)]], 'r_hip_2': [[(.075, .031) , 1.5304, eye(4)]], 'r_thigh': [[(.224, .034) , 1.5304, transl(0, 0, .112)], [(.077,.0315) , .79206, dot(transl(0, 0, .224), roty(pi/2))]], 'r_shank': [[(.213, .0315) , .95262, dot(transl(0, -.1065, 0), rotx(pi/2))]], 'r_ankle_1': [[(.063, .0245) , .14801, eye(4)]], 'r_foot': [[(.095, .027) , .59285, transl(0,0,-.0125)], [(.002, .027, .065), .08185, transl(-.039, 0, -.034)]], }
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 get_joints_data(): """ This dictionnary describes the joint of the robot Each joint has 3 arguments name : [parent_body, H_parentbody_jointframe, child_body] parent_body: the name parent body (a string) H_parentbody_jointframe: give the transformation matrix from the parent_body frame to the joint frame. The joint is always a RzJoint, a rotation around z axis. chid_body: the child body frame (a string) which is linked with the joint to the parent body. """ return { 'torso_pitch': ['waist', dot(rotz(pi / 2), roty(-pi / 2)), 'lap_belt_1'], 'torso_roll': ['lap_belt_1', T_DH(0, 0, .032, pi / 2), 'lap_belt_2'], 'torso_yaw': ['lap_belt_2', T_DH(0, -pi / 2, 0, pi / 2), 'chest'], 'head_pitch': ['chest', T_DH(-.1933, -pi / 2, .00231, -pi / 2), 'neck_1'], 'head_roll': ['neck_1', T_DH(0, pi / 2, .033, pi / 2), 'neck_2'], 'head_yaw': ['neck_2', T_DH(.001, -pi / 2, 0, -pi / 2), 'head'], 'l_shoulder_pitch': [ 'chest', T_DH(-.1433, 105 * pi / 180, .0233647, -pi / 2), 'l_shoulder_1' ], 'l_shoulder_roll': ['l_shoulder_1', T_DH(.10774, pi / 2, 0, -pi / 2), 'l_shoulder_2'], 'l_shoulder_yaw': ['l_shoulder_2', T_DH(0, -pi / 2, 0, pi / 2), 'l_arm'], 'l_elbow_pitch': ['l_arm', T_DH(.15228, 75 * pi / 180, 0, -pi / 2), 'l_elbow_1'], 'l_elbow_yaw': ['l_elbow_1', T_DH(0, 0, -.015, pi / 2), 'l_forearm'], 'l_wrist_roll': ['l_forearm', T_DH(.1373, -pi / 2, 0, pi / 2), 'l_wrist_1'], 'l_wrist_pitch': ['l_wrist_1', T_DH(0, pi / 2, 0, pi / 2), 'l_hand'], 'r_shoulder_pitch': [ 'chest', T_DH(-.1433, -105 * pi / 180, -.0233647, pi / 2), 'r_shoulder_1' ], 'r_shoulder_roll': ['r_shoulder_1', T_DH(-.10774, -pi / 2, 0, pi / 2), 'r_shoulder_2'], 'r_shoulder_yaw': ['r_shoulder_2', T_DH(0, -pi / 2, 0, -pi / 2), 'r_arm'], 'r_elbow_pitch': ['r_arm', T_DH(-.15228, -105 * pi / 180, 0, -pi / 2), 'r_elbow_1'], 'r_elbow_yaw': ['r_elbow_1', T_DH(0, 0, .015, pi / 2), 'r_forearm'], 'r_wrist_roll': ['r_forearm', T_DH(-.1373, -pi / 2, 0, pi / 2), 'r_wrist_1'], 'r_wrist_pitch': ['r_wrist_1', T_DH(0, pi / 2, 0, pi / 2), 'r_hand'], 'l_hip_pitch': ['waist', dot(transl(0, -.0681, -.1199), rotx(-pi / 2)), 'l_hip_1'], 'l_hip_roll': ['l_hip_1', T_DH(0, pi / 2, 0, -pi / 2), 'l_hip_2'], 'l_hip_yaw': ['l_hip_2', T_DH(0, pi / 2, 0, -pi / 2), 'l_thigh'], 'l_knee': ['l_thigh', T_DH(-.2236, -pi / 2, 0, pi / 2), 'l_shank'], 'l_ankle_pitch': ['l_shank', T_DH(0, pi / 2, -.213, pi), 'l_ankle_1'], 'l_ankle_roll': ['l_ankle_1', T_DH(0, 0, 0, -pi / 2), 'l_foot'], 'r_hip_pitch': [ 'waist', dot(transl(0, .0681, -.1199), rotx(-pi / 2)), 'r_hip_1' ], 'r_hip_roll': ['r_hip_1', T_DH(0, pi / 2, 0, pi / 2), 'r_hip_2'], 'r_hip_yaw': ['r_hip_2', T_DH(0, pi / 2, 0, pi / 2), 'r_thigh'], 'r_knee': ['r_thigh', T_DH(.2236, -pi / 2, 0, -pi / 2), 'r_shank'], 'r_ankle_pitch': ['r_shank', T_DH(0, pi / 2, -.213, pi), 'r_ankle_1'], 'r_ankle_roll': ['r_ankle_1', T_DH(0, 0, 0, pi / 2), 'r_foot'], }
def get_contact_data(): """ somes extras: give some other important frames Each subframe is described as follows: subframe name : [body frame, dims, H_body_subframe] they can be used for contact point for example """ return { 'lh1' : ['l_hand', (.012,), transl(.012, .0205, 0.)], 'lh2' : ['l_hand', (.012,), transl(.012, -.0205, 0.)], 'lh3' : ['l_hand', (.012,), transl(.057, .0205, 0.)], 'lh4' : ['l_hand', (.012,), transl(.057, -.0205, 0.)], 'l_hand_tip' : ['l_hand', (.012,), transl(.069, 0, 0)], 'l_hand_palm' : ['l_hand', (.012,), transl(.0345, 0, 0)], 'rh1' : ['r_hand', (.012,), transl(-.012, .0205, 0.)], 'rh2' : ['r_hand', (.012,), transl(-.012, -.0205, 0.)], 'rh3' : ['r_hand', (.012,), transl(-.057, .0205, 0.)], 'rh4' : ['r_hand', (.012,), transl(-.057, -.0205, 0.)], 'r_hand_tip' : ['r_hand', (.012,), transl(-.069, 0, 0)], 'r_hand_palm' : ['r_hand', (.012,), transl(-.0345, 0, 0)], 'lf1' : ['l_foot', (.002,), transl(-.039,-.027,-.031)], 'lf2' : ['l_foot', (.002,), transl(-.039, .027,-.031)], 'lf3' : ['l_foot', (.002,), transl(-.039, .027, .099)], 'lf4' : ['l_foot', (.002,), transl(-.039,-.027, .099)], 'l_sole': ['l_foot', (), dot(transl(-.041, .0 , .034), dot(roty(-pi/2), rotx(-pi/2) ) )], 'rf1' : ['r_foot', (.002,), transl(-.039,-.027, .031)], 'rf2' : ['r_foot', (.002,), transl(-.039, .027, .031)], 'rf3' : ['r_foot', (.002,), transl(-.039, .027,-.099)], 'rf4' : ['r_foot', (.002,), transl(-.039,-.027,-.099)], 'r_sole': ['r_foot', (), dot(transl(-.041, .0 ,-.034), dot(roty(pi/2), rotx(pi/2) ) )], }
error.append(sh-th) return error def plot_error(self): plot(self.timeline, self.get_error()) def plot_height(self): 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)
# 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 # 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()
def add_groundplane(w, half_extents=(1., .1, 1.) ): """Add a ground plane using a box shape. """ frame = SubFrame(w.ground, transl(0., -half_extents[1], 0.) ) w.register(Box(frame, half_extents, 'Ground shape'))
def add_groundplane(w, length=(1., .1, 1.) ): """Add a ground plane using a box shape. """ frame = SubFrame(w.ground, transl(0., -length[1]/2., 0.) ) w.register(Box(frame, length, 'Ground shape'))
def _human36(world, height=1.741, mass=73, name=''): """ TODO: HuMAnS' doc about inertia is erroneous (the real math is in the IOMatrix proc in DynamicData.maple) """ assert isinstance(world, World) w = world L = anatomical_lengths(height) bodies = {} humansbodyid_to_humansbodyname_map = {} for b in _humans_bodies(height, mass): #mass matrix at com mass_g = b['Mass'] * diag( hstack((b['GyrationRadius']**2, (1,1,1)))) H_fg = eye(4) H_fg[0:3,3] = b['CenterOfMass'] H_gf = Hg.inv(H_fg) #mass matrix at body's frame origin: mass_o = dot(adjoint(H_gf).T, dot(mass_g, adjoint(H_gf))) bodies[b['HumansName']] = Body( name=b['HumansName'], mass=mass_o) humansbodyid_to_humansbodyname_map[b['HumansId']] = b['HumansName'] rf = SubFrame(w.ground, Hg.transl(0, L['yfootL']+L['ytibiaL']+L['yfemurL'], 0)) w.add_link(rf, FreeJoint(), bodies['LPT']) rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, L['zhip']/2.)) j = RzRyRxJoint() w.add_link(rf, j, bodies['ThighR']) rf = SubFrame(bodies['ThighR'], Hg.transl(0, -L['yfemurR'], 0)) w.add_link(rf, RzJoint(), bodies['ShankR']) rf = SubFrame(bodies['ShankR'], Hg.transl(0, -L['ytibiaR'], 0)) w.add_link(rf, RzRxJoint(), bodies['FootR']) rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, -L['zhip']/2.)) w.add_link(rf, RzRyRxJoint(), bodies['ThighL']) rf = SubFrame(bodies['ThighL'], Hg.transl(0, -L['yfemurL'], 0)) w.add_link(rf, RzJoint(), bodies['ShankL']) rf = SubFrame(bodies['ShankL'], Hg.transl(0, -L['ytibiaL'], 0)) w.add_link(rf, RzRxJoint(), bodies['FootL']) rf = SubFrame(bodies['LPT'], Hg.transl(-L['xvT10'], L['yvT10'], 0)) w.add_link(rf, RzRyRxJoint(), bodies['UPT']) rf = SubFrame(bodies['UPT'], Hg.transl(L['xsternoclavR'], L['ysternoclavR'], L['zsternoclavR'])) j = RyRxJoint() w.add_link(rf, j, bodies['ScapulaR']) rf = SubFrame(bodies['ScapulaR'], Hg.transl(-L['xshoulderR'], L['yshoulderR'], L['zshoulderR'])) w.add_link(rf, RzRyRxJoint(), bodies['ArmR']) rf = SubFrame(bodies['ArmR'], Hg.transl(0, -L['yhumerusR'], 0)) w.add_link(rf, RzRyJoint(), bodies['ForearmR']) rf = SubFrame(bodies['ForearmR'], Hg.transl(0, -L['yforearmR'], 0)) w.add_link(rf, RzRxJoint(), bodies['HandR']) rf = SubFrame(bodies['UPT'], Hg.transl( L['xsternoclavL'], L['ysternoclavL'], -L['zsternoclavL'])) w.add_link(rf, RyRxJoint(), bodies['ScapulaL']) rf = SubFrame(bodies['ScapulaL'], Hg.transl(-L['xshoulderL'], L['yshoulderL'], -L['zshoulderL'])) w.add_link(rf, RzRyRxJoint(), bodies['ArmL']) rf = SubFrame(bodies['ArmL'], Hg.transl(0, -L['yhumerusL'], 0)) w.add_link(rf, RzRyJoint(), bodies['ForearmL']) rf = SubFrame(bodies['ForearmL'], Hg.transl(0, -L['yforearmL'], 0)) w.add_link(rf, RzRxJoint(), bodies['HandL']) rf = SubFrame(bodies['UPT'], Hg.transl(L['xvT10'], L['yvC7'], 0)) w.add_link(rf, RzRyRxJoint(), bodies['Head']) # add tags tags = {} for t in _humans_tags(height): bodyname = humansbodyid_to_humansbodyname_map[t['HumansBodyId']] tag = SubFrame( bodies[bodyname], Hg.transl(t['Position'][0], t['Position'][1], t['Position'][2]), t['HumansName']) tags[t['HumansName']] = tag w.register(tag) # Add point shapes to the feet for k in ('Right foot toe tip', 'Right foot heel', 'Right foot phalange 5', 'Right foot Phalange 1', 'Left foot toe tip','Left foot heel', 'Left foot phalange 5','Left foot phalange 1'): shape = Point(tags[k], name=k) w.register(shape) w.init() return (bodies, tags)
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 # # # ######################################### ## TASKS from LQPctrl.task import FrameTask
vel = self.parentjoint.gvel force = self.kp*(posdes-pos) - self.kd*vel gforce[self.parentjoint.dof] = force gforce[self.parentjoint.dof] += self.mass*9.81 return gforce, impedance ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_icub_and_init(gravity=True) add_table(w, hl=(.05,.2,.02), init_pos=transl(-.3,0,.6), mass=10., name='wall') joints = w.getjoints() frames = w.getframes() bodies = w.getbodies() shapes = w.getshapes() consts = w.getconstraints() ##POS of the arms, before or after the rec of standing_pos? joints['l_shoulder_pitch'].gpos[:] = -0.65695599 joints['l_shoulder_roll'].gpos[:] = 0.58153898 joints['l_shoulder_yaw'].gpos[:] = 0.68249881 joints['l_elbow_pitch'].gpos[:] = 1.27058836 joints['l_elbow_yaw'].gpos[:] = 0.85522174 joints['l_wrist_roll'].gpos[:] = 0.15702068 joints['l_wrist_pitch'].gpos[:] = 0.41882797
def pose(self): return Hg.transl(0., self.gpos[0], 0.)
const = w.getconstraints() ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import ForceTask, FrameTask from LQPctrl.task_ctrl import ValueCtrl, KpCtrl from arboris.homogeneousmatrix import rotz, transl from numpy import pi tasks = [] goal = rotz(pi / 8) goal[0:3, 3] = [-0.4, .5, 0] goal = transl(-.8, -.5, 0) tasks.append( ForceTask(const["const"], ValueCtrl([0, 0.001, .01]), [], 1., 0, True)) ## EVENTS events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2} lqpc = LQPcontroller(gforcemax, tasks=tasks) w.register(lqpc) ############################ # #
Matrices and vectors are representeed with numpy arrays. Here, we present some functions and operations to do mechanics. """ import numpy as np np.set_printoptions(suppress=True) ##### About homogeneous matrix ################################################# import arboris.homogeneousmatrix as Hg H_o_a = Hg.transl(1,2,3) # get a translation homogeneous matrix H_a_b = Hg.rotzyx(.1,.2,.3) # get a rotation R = Rz * Ry * Rx # also Hg.rotzy R = Rz * Ry # Hg.rotzx R = Rz * Rx # Hg.rotyx R = Ry * Rx # Hg.rotz R = Rz # Hg.roty R = Ry # Hg.rotx R = Rx H_o_b = np.dot(H_o_a, H_a_b) # H from {o} to {b} is H{o}->{a} * H{a}->{b} H_b_o = Hg.inv(H_o_b) # obtain inverse, H from {b} to {o} print("H_o_b * H_b_o\n", np.dot(H_o_b, H_b_o))
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])
error.append(sh - th) return error def plot_error(self): plot(self.timeline, self.get_error()) 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)
def pose(self): return Hg.transl(self.gpos[0], self.gpos[1], self.gpos[2])
def get_bodies_data(): """ This dictionnary describes the bodies of the robot Each body is a list of shape name : [ [(dims1) | mass1 | H1], [(dims2) | mass2 | H2] ] about dims (in meter): if dims has 1 element: it is a sphere; get (radius,) if dims has 2 elements: it is a cylinder with z-axis; get (length, radius) if dims has 3 elements: it is a box; get half-lengths along (x,y,z) masses are in kg about transformation matrix H: it represents the transformation from the body frame to the center of the shape. transl for translation along (x,y,z) (meter) rotx,roty, rotz for rotation around x,y,z axis dot for matrices multiplication """ return { ### WARNING: pb between shape and inertia in the c++ file of the simulator #'waist' : [[(0.002, 0.065, 0.027), 0.20297, dot(transl(0, 0, -0.065), rotx(pi/2))]], 'waist': [[(.032, .0235, .057215), .20297, transl(.006, 0, -.1)]], 'lap_belt_1': [[(.0315, .0635, .088), 3.623, dot(transl(-.0383, -.0173, 0), rotz(-pi / 4))]], 'lap_belt_2': [[(.097, .031), .91179, transl(0, 0, .008)]], 'chest': [[(.0274, .04), .45165, transl(0, 0, -.0447)], [(.036, .0545, .059), 1.8388, dot(transl(-.03, 0, -.1174), rotz(pi / 12))], [(.036, .0545, .059), 1.8388, dot(transl(.03, 0, -.1174), rotz(-pi / 12))]], 'neck_1': [[(.077, .015), .28252, eye(4)]], 'neck_2': [[(.033, .015), .1, eye(4)]], 'head': [[(.08, ), .78, transl(0, 0, .0825)]], 'l_shoulder_1': [[(.011, .031), .48278, transl(0, 0, .07224)]], 'l_shoulder_2': [[(.059, .03), .20779, eye(4)]], 'l_arm': [[(.156, .026), 1.1584, transl(0, 0, .078)]], 'l_elbow_1': [[(.01, ), .050798, eye(4)]], 'l_forearm': [[(.14, .02), .48774, transl(0, 0, .07)]], 'l_wrist_1': [[(.04, .01), .05, eye(4)]], 'l_hand': [[(.0345, .0325, .012), .19099, transl(.0345, 0, 0)]], 'r_shoulder_1': [[(.011, .031), .48278, transl(0, 0, -.07224)]], 'r_shoulder_2': [[(.059, .03), .20779, eye(4)]], 'r_arm': [[(.156, .026), 1.1584, transl(0, 0, -.078)]], 'r_elbow_1': [[(.01, ), .050798, eye(4)]], 'r_forearm': [[(.14, .02), .48774, transl(0, 0, -.07)]], 'r_wrist_1': [[(.04, .01), .05, eye(4)]], 'r_hand': [[(.0345, .0325, .012), .19099, transl(-.0345, 0, 0)]], 'l_hip_1': [[(.013, .038), .32708, transl(0, 0, .0375)]], 'l_hip_2': [[(.075, .031), 1.5304, eye(4)]], 'l_thigh': [[(.224, .034), 1.5304, transl(0, 0, -.112)], [(.077, .0315), .79206, dot(transl(0, 0, -.224), roty(pi / 2))]], 'l_shank': [[(.213, .0315), .95262, dot(transl(0, -.1065, 0), rotx(pi / 2))]], 'l_ankle_1': [[(.063, .0245), .14801, eye(4)]], 'l_foot': [[(.095, .027), .59285, transl(0, 0, .0125)], [(.002, .027, .065), .08185, transl(-.039, 0, .034)]], 'r_hip_1': [[(.013, .038), .32708, transl(0, 0, -.0375)]], 'r_hip_2': [[(.075, .031), 1.5304, eye(4)]], 'r_thigh': [[(.224, .034), 1.5304, transl(0, 0, .112)], [(.077, .0315), .79206, dot(transl(0, 0, .224), roty(pi / 2))]], 'r_shank': [[(.213, .0315), .95262, dot(transl(0, -.1065, 0), rotx(pi / 2))]], 'r_ankle_1': [[(.063, .0245), .14801, eye(4)]], 'r_foot': [[(.095, .027), .59285, transl(0, 0, -.0125)], [(.002, .027, .065), .08185, transl(-.039, 0, -.034)]], }
def ipose(self): return Hg.transl(-self.gpos[0], 0., 0.)
def get_contact_data(): """ somes extras: give some other important frames Each subframe is described as follows: subframe name : [body frame, dims, H_body_subframe] they can be used for contact point for example """ return { 'lh1': ['l_hand', (.012, ), transl(.012, .0205, 0.)], 'lh2': ['l_hand', (.012, ), transl(.012, -.0205, 0.)], 'lh3': ['l_hand', (.012, ), transl(.057, .0205, 0.)], 'lh4': ['l_hand', (.012, ), transl(.057, -.0205, 0.)], 'l_hand_tip': ['l_hand', (.012, ), transl(.069, 0, 0)], 'l_hand_palm': ['l_hand', (.012, ), transl(.0345, 0, 0)], 'rh1': ['r_hand', (.012, ), transl(-.012, .0205, 0.)], 'rh2': ['r_hand', (.012, ), transl(-.012, -.0205, 0.)], 'rh3': ['r_hand', (.012, ), transl(-.057, .0205, 0.)], 'rh4': ['r_hand', (.012, ), transl(-.057, -.0205, 0.)], 'r_hand_tip': ['r_hand', (.012, ), transl(-.069, 0, 0)], 'r_hand_palm': ['r_hand', (.012, ), transl(-.0345, 0, 0)], 'lf1': ['l_foot', (.002, ), transl(-.039, -.027, -.031)], 'lf2': ['l_foot', (.002, ), transl(-.039, .027, -.031)], 'lf3': ['l_foot', (.002, ), transl(-.039, .027, .099)], 'lf4': ['l_foot', (.002, ), transl(-.039, -.027, .099)], 'l_sole': [ 'l_foot', (), dot(transl(-.041, .0, .034), dot(roty(-pi / 2), rotx(-pi / 2))) ], 'rf1': ['r_foot', (.002, ), transl(-.039, -.027, .031)], 'rf2': ['r_foot', (.002, ), transl(-.039, .027, .031)], 'rf3': ['r_foot', (.002, ), transl(-.039, .027, -.099)], 'rf4': ['r_foot', (.002, ), transl(-.039, -.027, -.099)], 'r_sole': [ 'r_foot', (), dot(transl(-.041, .0, -.034), dot(roty(pi / 2), rotx(pi / 2))) ], }
self.f = f def init(self, world, timeline): pass 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()] standing_pose = [j.gpos[0] for j in icub_joints] ######################################### # # # CREATE TASKS, EVENTS & LQP controller #
def add_simplearm(world, name='', lengths=(0.5 ,0.4 , 0.2), masses=(1.0, 0.8, 0.2)): """Build a planar 3-R robot. TODO: make use of the ``name`` input argument to prefix bodies and joints names Example: >>> w = World() >>> add_simplearm(w) >>> w.update_dynamic() """ assert isinstance(world, World) w = world arm_length = lengths[0] arm_mass = masses[0] forearm_length = lengths[1] forearm_mass = masses[1] hand_length = lengths[2] hand_mass = masses[2] # create bodies arm = Body( name=name+'Arm', mass=transport_mass_matrix( mass_parallelepiped( arm_mass, (arm_length/10,arm_length,arm_length/10)), Hg.transl(0,arm_length/2,0))) forearm = Body( name=name+'ForeArm', mass=transport_mass_matrix( mass_parallelepiped( forearm_mass, (forearm_length/10,forearm_length,forearm_length/10)), Hg.transl(0,forearm_length/2,0))) hand = Body( name=name+'Hand', mass=transport_mass_matrix( mass_parallelepiped( hand_mass, (hand_length/10,hand_length,hand_length/10)), Hg.transl(0,hand_length/2,0))) # create a joint between the ground and the arm shoulder = RzJoint(name=name+'Shoulder') w.add_link(w.ground, shoulder, arm) # add a frame to the arm, where the forearm will be anchored f = SubFrame(arm, Hg.transl(0,arm_length,0), name+'ElbowBaseFrame') # create a joint between the arm and the forearm elbow = RzJoint(name=name+'Elbow') w.add_link(f, elbow, forearm) # add a frame to the forearm, where the hand will be anchored f = SubFrame(forearm, Hg.transl(0,forearm_length,0), name+'WristBaseFrame') # create a joint between the forearm and the hand wrist = RzJoint(name=name+'Wrist') w.add_link(f, wrist, hand) # create a frame at the end of the hand f = SubFrame(hand, Hg.transl(0,hand_length,0), name+'EndEffector') w.register(f) w.init()
######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import ForceTask, FrameTask from LQPctrl.task_ctrl import ValueCtrl, KpCtrl from arboris.homogeneousmatrix import rotz, transl from numpy import pi tasks = [] goal = rotz(pi/8) goal[0:3,3] = [-0.4, .5, 0] goal = transl(-.8,-.5,0) tasks.append(ForceTask( const["const"], ValueCtrl([0, 0.001,.01]) , [], 1., 0, True)) ## EVENTS events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = {"Shoulder":10,"Elbow":5,"Wrist":2} lqpc = LQPcontroller(gforcemax, tasks=tasks) w.register(lqpc)
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])
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.controllers import WeightController w.register(WeightController()) ##### Add observers from arboris.observers import PerfMonitor, Hdf5Logger from arboris.visu.dae_writer import write_collada_scene, write_collada_animation, add_shapes_to_dae from arboris.visu import pydaenimCom flat = False write_collada_scene(w, "./scene.dae", flat=flat) shapes_info = [ ["Arm" , "./dae/icub_simple.dae#head"], # parent frame, child shape node id ["Forearm" , "./dae/icub_simple.dae#head", transl(0,0.3,0)], # parent frame, child shape node id, H_frame_shape ["EndEffector", "./dae/icub_simple.dae" , transl(0,0.3,0), (0.3,0.3,0.3)], # parent frame, child shape file , H_frame_shape, shape_scale ] add_shapes_to_dae("./scene.dae", shapes_info) # add argument output_file="out.dae" if you want to keep original dae file obs = [] pobs = PerfMonitor(True) dobs = pydaenimCom("./scene.dae", flat=flat) h5obs = Hdf5Logger("sim.h5", mode="w", flat=flat) obs.append(pobs) obs.append(dobs) obs.append(h5obs)
force = self.kp * (posdes - pos) - self.kd * vel gforce[self.parentjoint.dof] = force gforce[self.parentjoint.dof] += self.mass * 9.81 return gforce, impedance ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_icub_and_init(gravity=True) add_table(w, hl=(.05, .2, .02), init_pos=transl(-.3, 0, .6), mass=10., name='wall') joints = w.getjoints() frames = w.getframes() bodies = w.getbodies() shapes = w.getshapes() consts = w.getconstraints() ##POS of the arms, before or after the rec of standing_pos? joints['l_shoulder_pitch'].gpos[:] = -0.65695599 joints['l_shoulder_roll'].gpos[:] = 0.58153898 joints['l_shoulder_yaw'].gpos[:] = 0.68249881 joints['l_elbow_pitch'].gpos[:] = 1.27058836 joints['l_elbow_yaw'].gpos[:] = 0.85522174
def get_meshes_positions(): return { "head_mesh" : dot(transl(0,0,-0.055), rotzyx(pi/2., 0, 0)), "torso_mesh" : dot(transl(0,0.024,-0.144), rotzyx(-pi/2., pi, 0)), "pelvis_mesh" : dot(transl(0.031,0.04,0), rotzyx(pi/2.,pi,pi/2.)), "l_upperleg_mesh": dot(transl(0,0,-.05), rotzyx( pi/2., 0, 0)), "r_upperleg_mesh": dot(transl(0,0, .05), rotzyx(-pi/2., pi, 0)), "l_lowerleg_mesh": rotzyx(pi,0, pi/2.), "r_lowerleg_mesh": rotzyx(pi,0, pi/2.), "l_foot_mesh" : dot(transl(-0.041,0,-0.03), rotzyx(0, pi/2.,0)), "r_foot_mesh" : dot(transl(-0.041,0, 0.03), rotzyx(pi,-pi/2.,0)), "l_upperarm_mesh": dot(transl(0,0.01, 0.14), rotzyx(-pi/2.,0,-pi/2.)), "r_upperarm_mesh": dot(transl(0,0.01,-0.14), rotzyx(-pi/2.,0,-pi/2.)), "l_forearm_mesh" : dot(transl(0,0,-0.004), rotzyx(pi,0,-pi/2.)), "r_forearm_mesh" : dot(transl(0,0, 0.004), rotzyx(0,0,-pi/2.)), ## simple meshes "waist_mesh" : transl(.006, 0,-.1), "chest_cylinder_mesh": transl(0, 0, -.0447), "neck_1_mesh" : eye(4), "neck_2_mesh" : eye(4), "head_sphere_mesh": transl(0, 0, .0825), "l_shoulder_1_mesh": transl(0, 0, .07224), "l_shoulder_2_mesh": eye(4), "l_arm_cyl_mesh": transl(0, 0, .078), "l_elbow_1_mesh": eye(4), "l_forearm_cyl_mesh": transl(0, 0, .07), "l_wrist_1_mesh": eye(4), "l_hand_mesh": transl(.0345, 0, 0), "r_shoulder_1_mesh": transl(0, 0, -.07224), "r_shoulder_2_mesh": eye(4), "r_arm_cyl_mesh": transl(0, 0, -.078), "r_elbow_1_mesh": eye(4), "r_forearm_cyl_mesh":transl(0, 0, -.07), "r_wrist_1_mesh": eye(4), "r_hand_mesh": transl(-.0345, 0, 0), "l_hip_1_mesh": transl(0, 0, .0375), "l_hip_2_mesh": eye(4), "l_thigh_cyl1_mesh": transl(0, 0, -.112), "l_thigh_cyl2_mesh": dot(transl(0, 0, -.224), roty(pi/2)), "l_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)), "l_ankle_1_mesh": eye(4), "l_foot_cyl_mesh": transl(0,0,.0125), "r_hip_1_mesh": transl(0, 0, -.0375), "r_hip_2_mesh": eye(4), "r_thigh_cyl1_mesh": transl(0, 0, .112), "r_thigh_cyl2_mesh": dot(transl(0, 0, .224), roty(pi/2)), "r_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)), "r_ankle_1_mesh": eye(4), "r_foot_cyl_mesh": transl(0,0,-.0125), }