def testSoftFingerContact(self): world = World() add_sphere(world, name='ball0') add_sphere(world, name='ball1') bodies = world.getbodies() bodies['ball0'].parentjoint.gpos[1,3] = 2.5 bodies['ball0'].parentjoint.gvel[4] = -1. c = SoftFingerContact(world._shapes, 0.1) world.register(c) world.init() world.update_dynamic() dt = 0.001 world.update_controllers(dt) world.update_constraints(dt) world.integrate(dt) world.update_dynamic() self.assertListsAlmostEqual(c.jacobian, [ [ 0., 1., 0., 0., 0., 0., 0. , -1. , 0. , 0., 0., 0.], [-1., 0., 0., 0., 0., 1., -1.499, 0. , 0. , 0., 0., -1.], [ 0., 0., -1., -1., 0., 0., 0. , 0. , -1.499, 1., 0., 0.], [ 0., 0., 0., 0., 1., 0., 0. , 0. , 0. , 0., -1., 0.]]) self.assertListsAlmostEqual(bodies['ball0'].pose, [ [ 1. , 0. , 0. , 0. ], [ 0. , 1. , 0. , 2.499], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ] ])
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 testGeometricUpdate(self): w = World() add_simplearm(w) bodies = w.getbodies() arm = bodies['Arm'] forearm = bodies['ForeArm'] hand = bodies['Hand'] w.update_geometric() self.assertListsAlmostEqual(arm.pose, eye(4)) self.assertListsAlmostEqual(forearm.pose, [ [ 1. , 0. , 0. , 0. ], [ 0. , 1. , 0. , 0.5], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ] ]) self.assertListsAlmostEqual(hand.pose, [ [ 1. , 0. , 0. , 0. ], [ 0. , 1. , 0. , 0.9], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ] ])
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
#author=joseph salini """This tutorial presents the structure of Arboris-python. It presents the World, Body, Joint, SubFrame and Constraint classes. """ ##### About the World ########################################################## from arboris.core import World from arboris.robots import simplearm w = World() # create an instance of world. simplearm.add_simplearm(w) # add a simple arm robot # The world is composed of ... bodies = w.getbodies() # ... named list of bodies joints = w.getjoints() # ... named list of joints frames = w.getframes() # ... named list of frames (including bodies) shapes = w.getshapes() # ... named list of shapes consts = w.getconstraints() # ... named list of constraints ctrls = w.getcontrollers() # ... named list of controllers # And that's all! print("bodies", bodies) print("joints", joints) # These lists are named, so instances can be retrieved as follows: body_Arm = bodies["Arm"] joint_Shoulder = joints["Shoulder"] print("Arm", body_Arm) print("Shoulder", joint_Shoulder)
## WORLD BUILDING ######################################################## w = World() from arboris.robots import human36 human36.add_human36(w) w.update_dynamic() ######################################################## ## INIT ######################################################## joints = w.getjoints() bodies = w.getbodies() frames = w.getframes() shapes = w.getshapes() joints[0].gpos[0:3,3] = array([.1, .2, .3]) w.update_dynamic() ######################################################## ## CONTROLLERS ######################################################## from arboris.controllers import WeightController w.register(WeightController())
######################################################## ## WORLD BUILDING ######################################################## w = World() from arboris.robots import human36 human36.add_human36(w) w.update_dynamic() ######################################################## ## INIT ######################################################## joints = w.getjoints() bodies = w.getbodies() frames = w.getframes() shapes = w.getshapes() joints[0].gpos[0:3, 3] = array([.1, .2, .3]) w.update_dynamic() ######################################################## ## CONTROLLERS ######################################################## from arboris.controllers import WeightController w.register(WeightController()) ######################################################## ## OBSERVERS
def testDynamicUpdate(self): w = World() add_simplearm(w) joints = w.getjoints() joints[0].gpos[0] = 0.5 joints[0].gvel[0] = 2.5 joints[1].gpos[0] = 1.0 joints[1].gvel[0] = -1.0 joints[2].gpos[0] = 2.0/3.0 joints[2].gvel[0] = -0.5 w.update_dynamic() bodies = w.getbodies() self.assertListsAlmostEqual(bodies['Arm'].pose, [[ 0.87758256, -0.47942554, 0. , 0. ], [ 0.47942554, 0.87758256, 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]]) self.assertListsAlmostEqual(bodies['ForeArm'].pose, [[ 0.0707372 , -0.99749499, 0. , -0.23971277], [ 0.99749499, 0.0707372 , 0. , 0.43879128], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]]) self.assertListsAlmostEqual(bodies['Hand'].pose, [[-0.56122931, -0.82766035, 0. , -0.63871076], [ 0.82766035, -0.56122931, 0. , 0.46708616], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]]) self.assertListsAlmostEqual(bodies['ground'].jacobian, [[ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.]]) self.assertListsAlmostEqual(bodies['Arm'].jacobian, [[ 0., 0., 0.], [ 0., 0., 0.], [ 1., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.]]) self.assertListsAlmostEqual(bodies['ForeArm'].jacobian, [[ 0. , 0. , 0. ], [ 0. , 0. , 0. ], [ 1. , 1. , 0. ], [-0.27015115, 0. , 0. ], [ 0.42073549, 0. , 0. ], [ 0. , 0. , 0. ]]) self.assertListsAlmostEqual(bodies['Hand'].jacobian, [[ 0. , 0. , 0. ], [ 0. , 0. , 0. ], [ 1. , 1. , 1. ], [-0.26649313, -0.3143549 , 0. ], [ 0.7450519 , 0.24734792, 0. ], [ 0. , 0. , 0. ]]) self.assertListsAlmostEqual(bodies['ground'].twist, [ 0., 0., 0., 0., 0., 0.]) self.assertListsAlmostEqual(bodies['Arm'].twist, [ 0. , 0. , 2.5, 0. , 0. , 0. ]) self.assertListsAlmostEqual(bodies['ForeArm'].twist, [ 0., 0., 1.5, -0.67537788, 1.05183873, 0. ]) self.assertListsAlmostEqual(bodies['Hand'].twist, [ 0., 0., 1., -0.35187792, 1.61528183, 0. ]) self.assertListsAlmostEqual(w.viscosity, [[ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.]]) self.assertListsAlmostEqual(bodies['Arm'].mass, [[ 8.35416667e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.50000000e-01], [ 0.00000000e+00, 4.16666667e-04, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 8.35416667e-02, -2.50000000e-01, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -2.50000000e-01, 1.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00, 0.00000000e+00], [ 2.50000000e-01, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) self.assertListsAlmostEqual(bodies['ForeArm'].mass, [[ 4.27733333e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.60000000e-01], [ 0.00000000e+00, 2.13333333e-04, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 4.27733333e-02, -1.60000000e-01, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -1.60000000e-01, 8.00000000e-01, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 8.00000000e-01, 0.00000000e+00], [ 1.60000000e-01, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 8.00000000e-01]]) self.assertListsAlmostEqual(bodies['Hand'].mass, [[ 2.67333333e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.00000000e-02], [ 0.00000000e+00, 1.33333333e-05, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 2.67333333e-03, -2.00000000e-02, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -2.00000000e-02, 2.00000000e-01, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.00000000e-01, 0.00000000e+00], [ 2.00000000e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.00000000e-01]]) self.assertListsAlmostEqual(w.mass, [[ 0.55132061, 0.1538999 , 0.0080032 ], [ 0.1538999 , 0.09002086, 0.00896043], [ 0.0080032 , 0.00896043, 0.00267333]]) self.assertListsAlmostEqual(bodies['ground'].djacobian, [[ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.]]) self.assertListsAlmostEqual(bodies['Arm'].djacobian, [[ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.], [ 0., 0., 0.]]) self.assertListsAlmostEqual(bodies['ForeArm'].djacobian, [[ 0. , 0. , 0. ], [ 0. , 0. , 0. ], [ 0. , 0. , 0. ], [-0.42073549, 0. , 0. ], [-0.27015115, 0. , 0. ], [ 0. , 0. , 0. ]]) self.assertListsAlmostEqual(bodies['Hand'].djacobian, [[ 0. , 0. , 0. ], [ 0. , 0. , 0. ], [ 0. , 0. , 0. ], [-0.87022993, -0.12367396, 0. ], [-0.08538479, -0.15717745, 0. ], [ 0. , 0. , 0. ]]) self.assertListsAlmostEqual(bodies['ground'].nleffects, [[ 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., 0., 0.]]) self.assertListsAlmostEqual(bodies['Arm'].nleffects, [[ 0.00000000e+00, -1.04166667e-03, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 5.26041667e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 6.25000000e-01, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, -2.50000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -6.25000000e-01, 2.50000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]) self.assertListsAlmostEqual(bodies['ForeArm'].nleffects, [[ 0.00000000e+00, -3.20000000e-04, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 1.61600000e-02, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, -2.22261445e-18], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.40000000e-01, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, -1.20000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -2.40000000e-01, 1.20000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]) self.assertListsAlmostEqual(bodies['Hand'].nleffects, [[ 0.00000000e+00, -1.33333333e-05, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 6.73333333e-04, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, -1.10961316e-18], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.00000000e-02, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, -2.00000000e-01, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -2.00000000e-02, 2.00000000e-01, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]) self.assertListsAlmostEqual(w.nleffects, [[ 0.11838112, -0.15894538, -0.01490104], [ 0.27979997, 0.00247348, -0.00494696], [ 0.03230564, 0.00742044, 0. ]])
"""This tutorial presents the structure of Arboris-python. It presents the World, Body, Joint, SubFrame and Constraint classes. """ ##### About the World ########################################################## from arboris.core import World from arboris.robots import simplearm w = World() # create an instance of world. simplearm.add_simplearm(w) # add a simple arm robot # The world is composed of ... bodies = w.getbodies() # ... named list of bodies joints = w.getjoints() # ... named list of joints frames = w.getframes() # ... named list of frames (including bodies) shapes = w.getshapes() # ... named list of shapes consts = w.getconstraints() # ... named list of constraints ctrls = w.getcontrollers() # ... named list of controllers # And that's all! print("bodies", bodies) print("joints", joints) # These lists are named, so instances can be retrieved as follows: body_Arm = bodies["Arm"] joint_Shoulder = joints["Shoulder"] print("Arm", body_Arm) print("Shoulder", joint_Shoulder)
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 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]) )