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
