def testLinksReplacement(self):
w = World()
bulb = RyRxJoint(name='bulb')
stem = Body(name='stem', mass=eye(6)*0.1)
w.add_link(w.ground, bulb, stem)
new_bulb = RzRxJoint(name='bulb')
w.replace_joint(bulb, new_bulb) # By another joint
joints = w.getjoints()
self.assertEqual(len(joints), 1)
self.assertEqual(joints[0], new_bulb)
new_new_bulb = RzRyRxJoint(name='bulb')
w.replace_joint(new_bulb, w.ground, new_new_bulb, stem) # By a kinematic chain
joints = w.getjoints()
self.assertEqual(len(joints), 1)
self.assertEqual(joints[0], new_new_bulb)
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
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), [])
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 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
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())
##### Add observers
from arboris.observers import PerfMonitor, Hdf5Logger
def testConstructor(self):
w1 = World()
wc = WeightController()
j = w1.getjoints()
pdc = ProportionalDerivativeController(j)
"""
__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])
"""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)
# At the beginning, the world has one body corresponding to the galilean frame
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. ]])
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)
