class Test_simple_3R(unittest.TestCase): def setUp(self): self.w = World() add_simplearm(self.w) self.joints = self.w.getjoints() self.frames = self.w.getframes() self.w.update_dynamic() def simulate(self, options): self.lqpc = LQPcontroller(self.gforcemax, tasks=self.tasks, options=options, solver_options={"show_progress":False}) self.w.register(self.lqpc) simulate(self.w, arange(0, .04, .01), [])
""" __author__ = ("Joseph Salini <*****@*****.**>") from arboris.core import World, 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 = 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.register(BalanceController(world, [task])) from arboris.core import JointsList add_simplearm(world, name='Right') joints=world.getjoints() J=JointsList(joints[0:3])
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 timeline = arange(0, 1, 5e-3) simulate(w, timeline, [obs, Drawer(w)]) time = timeline[:-1] obs.plot_error() show()
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) # At the beginning, the world has one body corresponding to the galilean frame galilean_frame = w.ground
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 timeline = arange(0,1,5e-3) simulate(w, timeline, [obs, Drawer(w)]) time = timeline[:-1] obs.plot_error() show()
######################################################## 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 ########################################################
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) # At the beginning, the world has one body corresponding to the galilean frame