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_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
""" This thest_arboris_coonection is a test to create an Arboris observer to communicate with a html page. This page will be update to show the robot moving in a three.js instance... """ from arboris.core import World from arboris.robots import simplearm, icub ##### Create world w = World() #simplearm.add_simplearm(w, with_shapes=True) icub.add(w, create_shapes=False) w._up[:] = [0,0,1] ##### Init World joints = w.getjoints() # for n in ["Shoulder", "Elbow", "Wrist"]: # joints[n].gpos[0] = .5 ##### Add ctrl from arboris.controllers import WeightController w.register(WeightController())
from arboris.massmatrix import principalframe, transport from arboris.homogeneousmatrix import inv as Hinv from arboris.homogeneousmatrix import transl, roty, rotzyx, rotx import numpy numpy.set_printoptions(suppress=True) from numpy import dot, eye, pi ################################################################################ # # Create world with iCub # ################################################################################ w = World() icub.add(w) w.update_dynamic() jLim = icub.get_joint_limits(in_radian=True) tauLim = icub.get_torque_limits() # interesting values we want to obtain H_body_com = {} body_mass = {} body_moment_of_inertia = {} H_parentCoM_bCoM = {} list_of_dof = {} list_of_children = {} # We compute the displacement from the body frame to the body center of mass, aligned with the principal axis of inertia