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 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 create_3r_and_init(gpos=(0,0,0), gvel=(0,0,0), gravity=False): ## CREATE THE WORLD w = World() simplearm.add_simplearm(w, with_shapes=True) ## INIT joints = w.getjoints() joints["Shoulder"].gpos[:] = gpos[0] joints["Elbow"].gpos[:] = gpos[1] joints["Wrist"].gpos[:] = gpos[2] joints["Shoulder"].gvel[:] = gvel[0] joints["Elbow"].gvel[:] = gvel[1] joints["Wrist"].gvel[:] = gvel[2] w.update_dynamic() ## CTRL if gravity: w.register(WeightController()) return w
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()
#!/usr/bin/env python """ This example show how to add mesh from a collada file to another, in order to display complex meshes during visualization. """ from arboris.core import World from arboris.robots import simplearm from arboris.homogeneousmatrix import transl ##### Create world w = World() simplearm.add_simplearm(w, with_shapes=True) ##### Init joints = w.getjoints() for i in range(3): joints[i].gpos[:] = 0.5 w.update_dynamic() ##### Add ctrl from arboris.controllers import WeightController w.register(WeightController())
#!/usr/bin/python #coding=utf-8 #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)