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_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):
self.sarmw = World()
add_simplearm(self.sarmw)
self.alea_pose = array([ [ 0. , 5. , 0. , 0. ],
[ 2.6, 1. , 0. , 4.2],
[ 0. , 0. , 0. , 0. ],
[ 0. , 9. , 0. , 3. ] ])
self.alea_jac = array([ [ 4.2, 8., 1. ],
[ 0., 0., 0. ],
[ 0., 7., 0. ],
[ 2.6, 0., 0. ],
[ 0., 0., 0.5 ],
[ 8.8, 0., 2. ] ])
self.alea_djac = array([ [ 1., 0.8, 0. ],
[ 0., 42., 0. ],
[ 0., 4.2, 2.6 ],
[ 1., 0., 0. ],
[ 0., 1., 0. ],
[ 7.1, 6., 0. ] ])
self.alea_twist = [ 0., 0., 7.1, 1., 42, 0. ]
#!/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())
This example shows how to use BalanceController.
"""
__author__ = ("Joseph Salini <*****@*****.**>")
from arboris.core import ObservableWorld, 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 = ObservableWorld()
#world.observers.append(Drawer(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))
world.register(BalanceController(world, [task]))
from arboris.core import JointsList
add_simplearm(world, name='Right')
#!/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)
def simplearm():
from arboris.robots.simplearm import add_simplearm
w = World()
add_simplearm(w)
return w
from arboris.core import World, simulate, SubFrame from arboris.homogeneousmatrix import transl from numpy import array, arange, eye from arboris.controllers import WeightController from arboris.observers import Hdf5Logger time = arange(0, .05, 0.01) file_name = "the_temporary_file_where_we_save_simulation_data.h5" dest_in_file = "test/xp" world = World() world.register( WeightController() ) h5obs = Hdf5Logger(world, len(time)-1, file_name, dest_in_file, 'w') from arboris.robots import simplearm simplearm.add_simplearm(world) world.getjoints()[0].gpos = array([0.1]) simulate(world, time,[h5obs] ) import os os.remove(file_name)
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. ]])
def simplearm():
from arboris.robots.simplearm import add_simplearm
w = World()
add_simplearm(w)
return w
def setUp(self):
self.w = World()
add_simplearm(self.w)
self.joints = self.w.getjoints()
self.frames = self.w.getframes()
self.w.update_dynamic()
#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"]
