def add_simplearm(world, name='', lengths=(0.5 , 0.4 , 0.2),
masses=(1.0, 0.8, 0.2), with_shapes=False):
"""Build a planar 3-R robot.
***Example:***
>>> w = World()
>>> add_simplearm(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
def create_body(name, length, mass):
"""create a body"""
half_extents = (length/20., length/2., length/20.)
mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass)
mass_matrix_at_base = arboris.massmatrix.transport(
mass_matrix_at_com,
Hg.transl(0., -length/2., 0.))
body = Body(name, mass_matrix_at_base)
if with_shapes:
f = SubFrame(body, Hg.transl(0., length/2., 0.))
shape = arboris.shapes.Box(f, half_extents)
world.register(shape)
return body
# create the 3 bodies
arm = create_body(name+'Arm', lengths[0], masses[0])
forearm = create_body(name+'Forearm', lengths[1], masses[1])
hand = create_body(name+'Hand', lengths[2], masses[2])
# create a joint between the ground and the arm
shoulder = RzJoint(name=name+'Shoulder')
world.add_link(world.ground, shoulder, arm)
# add a frame to the arm, where the forearm will be anchored
f = SubFrame(arm,
Hg.transl(0, lengths[0], 0),
name+'ElbowBaseFrame')
# create a joint between the arm and the forearm
elbow = RzJoint(name=name+'Elbow')
world.add_link(f, elbow, forearm)
# add a frame to the forearm, where the hand will be anchored
f = SubFrame(forearm,
Hg.transl(0, lengths[1], 0),
name+'WristBaseFrame')
# create a joint between the forearm and the hand
wrist = RzJoint(name=name+'Wrist')
world.add_link(f, wrist, hand)
# create a frame at the end of the hand
f = SubFrame(hand, Hg.transl(0, lengths[2], 0), name+'EndEffector')
world.register(f)
world.init()
def add_simplearm(world,
name='',
lengths=(0.5, 0.4, 0.2),
masses=(1.0, 0.8, 0.2),
with_shapes=False):
"""Build a planar 3-R robot.
***Example:***
>>> w = World()
>>> add_simplearm(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
def create_body(name, length, mass):
"""create a body"""
half_extents = (length / 20., length / 2., length / 20.)
mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass)
mass_matrix_at_base = arboris.massmatrix.transport(
mass_matrix_at_com, Hg.transl(0., -length / 2., 0.))
body = Body(name, mass_matrix_at_base)
if with_shapes:
f = SubFrame(body, Hg.transl(0., length / 2., 0.))
shape = arboris.shapes.Box(f, half_extents)
world.register(shape)
return body
# create the 3 bodies
arm = create_body(name + 'Arm', lengths[0], masses[0])
forearm = create_body(name + 'Forearm', lengths[1], masses[1])
hand = create_body(name + 'Hand', lengths[2], masses[2])
# create a joint between the ground and the arm
shoulder = RzJoint(name=name + 'Shoulder')
world.add_link(world.ground, shoulder, arm)
# add a frame to the arm, where the forearm will be anchored
f = SubFrame(arm, Hg.transl(0, lengths[0], 0), name + 'ElbowBaseFrame')
# create a joint between the arm and the forearm
elbow = RzJoint(name=name + 'Elbow')
world.add_link(f, elbow, forearm)
# add a frame to the forearm, where the hand will be anchored
f = SubFrame(forearm, Hg.transl(0, lengths[1], 0), name + 'WristBaseFrame')
# create a joint between the forearm and the hand
wrist = RzJoint(name=name + 'Wrist')
world.add_link(f, wrist, hand)
# create a frame at the end of the hand
f = SubFrame(hand, Hg.transl(0, lengths[2], 0), name + 'EndEffector')
world.register(f)
world.init()
def get_joints_data():
""" This dictionnary describes the joint of the robot
Each joint has 3 arguments
name : [parent_body, H_parentbody_jointframe, child_body]
parent_body: the name parent body (a string)
H_parentbody_jointframe: give the transformation matrix from the
parent_body frame to the joint frame. The joint is
always a RzJoint, a rotation around z axis.
chid_body: the child body frame (a string) which is linked with
the joint to the parent body.
"""
return {
'torso_pitch': ['waist' , dot(rotz(pi/2), roty(-pi/2)), 'lap_belt_1'],
'torso_roll': ['lap_belt_1', T_DH(0, 0, .032, pi/2) , 'lap_belt_2'],
'torso_yaw': ['lap_belt_2', T_DH(0, -pi/2, 0, pi/2) , 'chest'],
'head_pitch': ['chest' , T_DH(-.1933, -pi/2, .00231, -pi/2), 'neck_1'],
'head_roll': ['neck_1', T_DH(0, pi/2, .033, pi/2) , 'neck_2'],
'head_yaw': ['neck_2', T_DH(.001, -pi/2, 0, -pi/2) , 'head'],
'l_shoulder_pitch': ['chest' , T_DH(-.1433, 105*pi/180, .0233647,-pi/2), 'l_shoulder_1'],
'l_shoulder_roll': ['l_shoulder_1', T_DH(.10774, pi/2, 0, -pi/2) , 'l_shoulder_2'],
'l_shoulder_yaw': ['l_shoulder_2', T_DH(0, -pi/2, 0, pi/2) , 'l_arm'],
'l_elbow_pitch': ['l_arm' , T_DH(.15228, 75*pi/180, 0, -pi/2) , 'l_elbow_1'],
'l_elbow_yaw': ['l_elbow_1' , T_DH(0, 0, -.015, pi/2) , 'l_forearm'],
'l_wrist_roll': ['l_forearm' , T_DH(.1373, -pi/2, 0, pi/2) , 'l_wrist_1'],
'l_wrist_pitch': ['l_wrist_1' , T_DH(0, pi/2, 0, pi/2) , 'l_hand'],
'r_shoulder_pitch': ['chest' , T_DH(-.1433,-105*pi/180,-.0233647, pi/2), 'r_shoulder_1'],
'r_shoulder_roll': ['r_shoulder_1', T_DH(-.10774, -pi/2, 0, pi/2) , 'r_shoulder_2'],
'r_shoulder_yaw': ['r_shoulder_2', T_DH(0, -pi/2, 0, -pi/2) , 'r_arm'],
'r_elbow_pitch': ['r_arm' , T_DH(-.15228, -105*pi/180, 0, -pi/2) , 'r_elbow_1'],
'r_elbow_yaw': ['r_elbow_1' , T_DH(0, 0, .015, pi/2) , 'r_forearm'],
'r_wrist_roll': ['r_forearm' , T_DH(-.1373, -pi/2, 0, pi/2) , 'r_wrist_1'],
'r_wrist_pitch': ['r_wrist_1' , T_DH(0, pi/2, 0, pi/2) , 'r_hand'],
'l_hip_pitch': ['waist' , dot(transl(0,-.0681, -.1199), rotx(-pi/2)), 'l_hip_1'],
'l_hip_roll': ['l_hip_1' , T_DH(0, pi/2, 0, -pi/2) , 'l_hip_2'],
'l_hip_yaw': ['l_hip_2' , T_DH(0, pi/2, 0, -pi/2) , 'l_thigh'],
'l_knee': ['l_thigh' , T_DH(-.2236 , -pi/2, 0, pi/2) , 'l_shank'],
'l_ankle_pitch': ['l_shank' , T_DH(0 , pi/2, -.213 , pi) , 'l_ankle_1'],
'l_ankle_roll': ['l_ankle_1', T_DH(0, 0, 0, -pi/2) , 'l_foot'],
'r_hip_pitch': ['waist' , dot(transl(0, .0681, -.1199), rotx(-pi/2)), 'r_hip_1'],
'r_hip_roll': ['r_hip_1' , T_DH(0, pi/2, 0, pi/2) , 'r_hip_2'],
'r_hip_yaw': ['r_hip_2' , T_DH(0, pi/2, 0, pi/2) , 'r_thigh'],
'r_knee': ['r_thigh' , T_DH(.2236 , -pi/2, 0, -pi/2) , 'r_shank'],
'r_ankle_pitch': ['r_shank' , T_DH(0 , pi/2, -.213 , pi) , 'r_ankle_1'],
'r_ankle_roll': ['r_ankle_1', T_DH(0, 0, 0, pi/2) , 'r_foot'],
}
def create_body(name, length, mass):
"""create a body"""
half_extents = (length / 20., length / 2., length / 20.)
mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass)
mass_matrix_at_base = arboris.massmatrix.transport(
mass_matrix_at_com, Hg.transl(0., -length / 2., 0.))
body = Body(name, mass_matrix_at_base)
if with_shapes:
f = SubFrame(body, Hg.transl(0., length / 2., 0.))
shape = arboris.shapes.Box(f, half_extents)
world.register(shape)
return body
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_body(name, length, mass):
"""create a body"""
half_extents = (length/20., length/2., length/20.)
mass_matrix_at_com = arboris.massmatrix.box(half_extents, mass)
mass_matrix_at_base = arboris.massmatrix.transport(
mass_matrix_at_com,
Hg.transl(0., -length/2., 0.))
body = Body(name, mass_matrix_at_base)
if with_shapes:
f = SubFrame(body, Hg.transl(0., length/2., 0.))
shape = arboris.shapes.Box(f, half_extents)
world.register(shape)
return body
def get_bodies_shapes_data():
"""
"""
bodies_shapes_data = {}
for name, data in get_bodies_data().items():
bodies_shapes_data[name] = [[d[0], d[2]] for d in data]
bodies_shapes_data["head"].extend([
[(.02, ), transl(-.034, -.054, .0825)], #l_eye
[(.02, ), transl(.034, -.054, .0825)], #r_eye
])
return bodies_shapes_data
def get_bodies_shapes_data():
"""
"""
bodies_shapes_data = {}
for name, data in get_bodies_data().items():
bodies_shapes_data[name] = [[d[0], d[2]] for d in data]
bodies_shapes_data["head"].extend([
[(.02,), transl(-.034, -.054, .0825)], #l_eye
[(.02,), transl( .034, -.054, .0825)], #r_eye
])
return bodies_shapes_data
def add_snake(w,
nbody,
lengths=None,
masses=None,
gpos=None,
gvel=None,
is_fixed=True):
"""Add a snake-shape robot to the world.
**Example:**
>>> w = World()
>>> add_snake(w, 3, lengths=[1.5, 1., 5.])
"""
assert isinstance(w, World)
if lengths is None:
lengths = [.5] * nbody
assert nbody == len(lengths)
if masses is None:
masses = [2.] * nbody
assert nbody == len(masses)
if gpos is None:
gpos = [0.] * nbody
assert nbody == len(gpos)
if gvel is None:
gvel = [0.] * nbody
assert nbody == len(gvel)
if is_fixed:
frame = w.ground
else:
L = lengths[0] / 2.
body = Body(mass=box([L, L, L], masses[0]))
w.add_link(w.ground, FreeJoint(), body)
frame = body
for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
radius = length / 10. #TODO use 5 instead of 10
#angle = pi/2.
angle = 0. #TODO: remove, it is here for testing
body = Body(
mass=transport(cylinder(length, radius, mass),
dot(rotx(angle), transl(0., -length / 2., 0.))))
joint = RzJoint(gpos=q, gvel=dq)
w.add_link(frame, joint, body)
frame = SubFrame(body, transl(0., length, 0.))
w.register(frame)
w.init()
def convert_root_joint(self, world, root_joint):
assert isinstance(root_joint, Joint)
assert isinstance(world, World)
assert root_joint.frames[0].body is world.ground
if isinstance(root_joint, joints.FreeJoint):
# nothing to do
pass
else:
from arboris.homogeneousmatrix import transl
from arboris.massmatrix import box as boxmass
from arboris.shapes import Box, Sphere
from arboris.constraints import SoftFingerContact
# add a "free-floating" base body to the robot
base = Body(mass=boxmass(self.base_lengths, self.base_mass))
world.replace_joint(root_joint, root_joint.frames[0],
joints.FreeJoint(), base, base, root_joint,
root_joint.frames[1])
# add a ground plane
r = self.contact_radius
ground_half_extents = [d / 2. + 2 * r for d in self.base_lengths]
ground_plane = Box(world.ground, ground_half_extents)
world.register(ground_plane)
# put 4 spheres at the bottom of the base
(x, y, z) = self.base_lengths
for (i, j) in ((1, 1), (1, -1), (-1, -1), (-1, 1)):
sf = SubFrame(base, transl(i * x / 2, -y / 2, j * z / 2))
sh = Sphere(sf, r)
world.register(sh)
contact = SoftFingerContact((ground_plane, sh),
self.friction_coeff)
world.register(contact)
def convert_root_joint(self, world, root_joint):
assert isinstance(root_joint, Joint)
assert isinstance(world, World)
assert root_joint.frames[0].body is world.ground
if isinstance(root_joint, joints.FreeJoint):
# nothing to do
pass
else:
from arboris.homogeneousmatrix import transl
from arboris.massmatrix import box as boxmass
from arboris.shapes import Box, Sphere
from arboris.constraints import SoftFingerContact
# add a "free-floating" base body to the robot
base = Body(mass=boxmass(self.base_lengths, self.base_mass))
world.replace_joint(root_joint,
root_joint.frames[0],
joints.FreeJoint(),
base,
base,
root_joint,
root_joint.frames[1])
# add a ground plane
r = self.contact_radius
ground_lengths = [ d+4*r for d in self.base_lengths]
ground_plane = Box(world.ground, ground_lengths)
world.register(ground_plane)
# put 4 spheres at the bottom of the base
(x, y, z) = self.base_lengths
for (i, j) in ((1, 1), (1, -1), (-1, -1), (-1, 1)):
sf = SubFrame(base, transl(i*x/2, -y/2, j*z/2))
sh = Sphere(sf, r)
world.register(sh)
contact = SoftFingerContact((ground_plane, sh), self.friction_coeff)
world.register(contact)
def add_snake(w, nbody, lengths=None, masses=None, gpos=None, gvel=None,
is_fixed=True):
"""Add a snake-shape robot to the world.
**Example:**
>>> w = World()
>>> add_snake(w, 3, lengths=[1.5, 1., 5.])
"""
assert isinstance(w, World)
if lengths is None:
lengths = [.5] * nbody
assert nbody == len(lengths)
if masses is None:
masses = [2.] * nbody
assert nbody == len(masses)
if gpos is None:
gpos = [0.] * nbody
assert nbody == len(gpos)
if gvel is None:
gvel = [0.] * nbody
assert nbody == len(gvel)
if is_fixed:
frame = w.ground
else:
L = lengths[0]/2.
body = Body(mass=box([L, L, L], masses[0]))
w.add_link(w.ground, FreeJoint(), body)
frame = body
for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
radius = length/10. #TODO use 5 instead of 10
#angle = pi/2.
angle = 0. #TODO: remove, it is here for testing
body = Body(mass=transport(cylinder(length, radius, mass),
dot(rotx(angle),
transl(0., -length/2., 0.))))
joint = RzJoint(gpos=q, gvel=dq)
w.add_link(frame, joint, body)
frame = SubFrame(body, transl(0., length, 0.))
w.register(frame)
w.init()
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 add_tag(name, body, position):
"""Returns data about anatomical landmarks as defined in HuMAnS.
:param name: name of the tag (copied from HuMAnS)
:param body: name of the body in whose frame the point is defined
:position: tag coordinates in meters
"""
if name:
name = prefix + name
tag = SubFrame(bodies[prefix + body], Hg.transl(*position), name)
tags.append(tag)
w.register(tag)
def add_tag(name, body, position):
"""Returns data about anatomical landmarks as defined in HuMAnS.
:param name: name of the tag (copied from HuMAnS)
:param body: name of the body in whose frame the point is defined
:position: tag coordinates in meters
"""
if name:
name = prefix + name
tag = SubFrame(bodies[prefix+body], Hg.transl(*position), name)
tags.append(tag)
w.register(tag)
def add_robot(w, gpos, gvel, is_fixed=True):
assert isinstance(w, World)
if is_fixed:
frame = w.ground
else:
L = lengths[0]/2.
body = Body(mass=box([L, L, L], masses[0]))
w.add_link(w.ground, FreeJoint(), body)
frame = body
for (length, mass, q, dq) in zip(lengths, masses, gpos, gvel):
radius = length/10.
M = transport(cylinder(length, radius, mass),
transl(0., -length/2., 0.))
body = Body(mass=M)
joint = RzJoint(gpos=q, gvel=dq)
w.add_link(frame, joint, body)
frame = SubFrame(body, transl(0., length, 0.))
w.register(frame)
w.init()
def testLinksCreation(self):
w = World()
bulb = RyRxJoint(name='bulb')
stem = Body(name='stem', mass=eye(6)*0.1)
w.add_link(w.ground, bulb, stem)
stem_top = SubFrame(stem, bpose=transl(0, 0, 1), name='stem_top')
flowerR = RyJoint(gpos=.5, name='flowerR')
leafR = Body('leafR', mass=eye(6)*0.01)
w.add_link(stem_top, flowerR, leafR)
flowerL = RyJoint(gpos=(-.5), name='flowerL')
leafL = Body('leafL', mass=eye(6)*.01)
w.add_link(stem_top, flowerL, leafL)
frames = [ w.ground, stem, leafR, leafL, stem_top ]
for frame1, frame2 in zip(w.iterframes(), frames):
self.assertEqual(frame1, frame2)
joints = [ bulb, flowerR, flowerL ]
for joint1, joint2 in zip(w.iterjoints(), joints):
self.assertEqual(joint1, joint2)
"""
__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')
joints=world.getjoints()
N = b.nleffects B = b.viscosity # information about its position in kinematic structure p_joint = b.parentjoint # return the parent joint c_joints = b.childrenjoints # return list of chidren joints ##### The SubFrame/MovingSubFrame classes ###################################### from arboris.core import SubFrame, MovingSubFrame from arboris.homogeneousmatrix import transl # How to create a fixed subframe H_b_f = transl(.1,.2,.3) # homogeneous matrix from body to subframe sf = SubFrame(body=w.ground, bpose=H_b_f, name="myFrame") # bpose constant # How to create a moving subframe #Warning: these are not designed to create the kinematic tree. # they are intended to be used as target for example msf = MovingSubFrame(w.ground, name="myMovingFrame") msf.bpose = H_b_f # possible only with MovingSubFrame instance ##### The Joint class ########################################################## # A joint relates a parent frame with a child frame by constraining their # relative motion.
def get_bodies_data():
""" This dictionnary describes the bodies of the robot
Each body is a list of shape
name : [ [(dims1) | mass1 | H1],
[(dims2) | mass2 | H2] ]
about dims (in meter):
if dims has 1 element: it is a sphere; get (radius,)
if dims has 2 elements: it is a cylinder with z-axis; get (length, radius)
if dims has 3 elements: it is a box; get half-lengths along (x,y,z)
masses are in kg
about transformation matrix H:
it represents the transformation from the body frame to the center of
the shape.
transl for translation along (x,y,z) (meter)
rotx,roty, rotz for rotation around x,y,z axis
dot for matrices multiplication
"""
return {
### WARNING: pb between shape and inertia in the c++ file of the simulator
#'waist' : [[(0.002, 0.065, 0.027), 0.20297, dot(transl(0, 0, -0.065), rotx(pi/2))]],
'waist' : [[(.032, .0235, .057215), .20297, transl(.006, 0,-.1)]],
'lap_belt_1': [[(.0315, .0635, .088) , 3.623 , dot(transl(-.0383, -.0173, 0), rotz(-pi/4))]],
'lap_belt_2': [[(.097, .031) , .91179, transl(0, 0, .008)]],
'chest': [[(.0274, .04) , .45165, transl(0, 0, -.0447)],
[(.036, .0545, .059) , 1.8388, dot(transl(-.03, 0,-.1174), rotz( pi/12))],
[(.036, .0545, .059) , 1.8388, dot(transl( .03, 0,-.1174), rotz(-pi/12))]],
'neck_1': [[(.077, .015), .28252, eye(4)]],
'neck_2': [[(.033, .015), .1 , eye(4)]],
'head': [[(.08,) , .78 , transl(0, 0, .0825)]],
'l_shoulder_1': [[(.011, .031) , .48278 , transl(0, 0, .07224)]],
'l_shoulder_2': [[(.059, .03) , .20779 , eye(4)]],
'l_arm': [[(.156, .026) , 1.1584 , transl(0, 0, .078)]],
'l_elbow_1': [[(.01,) , .050798, eye(4)]],
'l_forearm': [[(.14, .02) , .48774 , transl(0, 0, .07)]],
'l_wrist_1': [[(.04, .01) , .05 , eye(4)]],
'l_hand': [[(.0345, .0325, .012), .19099 , transl(.0345, 0, 0)]],
'r_shoulder_1': [[(.011, .031) , .48278 , transl(0, 0, -.07224)]],
'r_shoulder_2': [[(.059, .03) , .20779 , eye(4)]],
'r_arm': [[(.156, .026) , 1.1584 , transl(0, 0, -.078)]],
'r_elbow_1': [[(.01,) , .050798, eye(4)]],
'r_forearm': [[(.14, .02) , .48774 , transl(0, 0, -.07)]],
'r_wrist_1': [[(.04, .01) , .05 , eye(4)]],
'r_hand': [[(.0345, .0325, .012), .19099 , transl(-.0345, 0, 0)]],
'l_hip_1': [[(.013, .038) , .32708, transl(0, 0, .0375)]],
'l_hip_2': [[(.075, .031) , 1.5304, eye(4)]],
'l_thigh': [[(.224, .034) , 1.5304, transl(0, 0, -.112)],
[(.077,.0315) , .79206, dot(transl(0, 0, -.224), roty(pi/2))]],
'l_shank': [[(.213, .0315) , .95262, dot(transl(0, -.1065, 0), rotx(pi/2))]],
'l_ankle_1': [[(.063, .0245) , .14801, eye(4)]],
'l_foot': [[(.095, .027) , .59285, transl(0,0,.0125)],
[(.002, .027, .065), .08185, transl(-.039, 0, .034)]],
'r_hip_1': [[(.013, .038) , .32708, transl(0, 0, -.0375)]],
'r_hip_2': [[(.075, .031) , 1.5304, eye(4)]],
'r_thigh': [[(.224, .034) , 1.5304, transl(0, 0, .112)],
[(.077,.0315) , .79206, dot(transl(0, 0, .224), roty(pi/2))]],
'r_shank': [[(.213, .0315) , .95262, dot(transl(0, -.1065, 0), rotx(pi/2))]],
'r_ankle_1': [[(.063, .0245) , .14801, eye(4)]],
'r_foot': [[(.095, .027) , .59285, transl(0,0,-.0125)],
[(.002, .027, .065), .08185, transl(-.039, 0, -.034)]],
}
from arboris.core import SubFrame
from arboris.shapes import Sphere, Cylinder
from arboris.homogeneousmatrix import transl
from arboris.constraints import SoftFingerContact
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_3r_and_init(gpos=(.5, .5, .5))
frames = w.getframes()
w.register(Sphere(frames['EndEffector'], radius=.02, name='ee'))
w.register(
Sphere(SubFrame(w.ground, transl(-.8, .2, 0.), name='obstacle'),
radius=.1,
name='obstacle'))
w.register(
Sphere(SubFrame(w.ground, transl(-.55, .2, 0.), name='obstacle2'),
radius=.1,
name='obstacle2'))
shapes = w.getshapes()
#w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01))
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
def get_joints_data():
""" This dictionnary describes the joint of the robot
Each joint has 3 arguments
name : [parent_body, H_parentbody_jointframe, child_body]
parent_body: the name parent body (a string)
H_parentbody_jointframe: give the transformation matrix from the
parent_body frame to the joint frame. The joint is
always a RzJoint, a rotation around z axis.
chid_body: the child body frame (a string) which is linked with
the joint to the parent body.
"""
return {
'torso_pitch':
['waist', dot(rotz(pi / 2), roty(-pi / 2)), 'lap_belt_1'],
'torso_roll': ['lap_belt_1',
T_DH(0, 0, .032, pi / 2), 'lap_belt_2'],
'torso_yaw': ['lap_belt_2',
T_DH(0, -pi / 2, 0, pi / 2), 'chest'],
'head_pitch':
['chest', T_DH(-.1933, -pi / 2, .00231, -pi / 2), 'neck_1'],
'head_roll': ['neck_1',
T_DH(0, pi / 2, .033, pi / 2), 'neck_2'],
'head_yaw': ['neck_2',
T_DH(.001, -pi / 2, 0, -pi / 2), 'head'],
'l_shoulder_pitch': [
'chest',
T_DH(-.1433, 105 * pi / 180, .0233647, -pi / 2), 'l_shoulder_1'
],
'l_shoulder_roll':
['l_shoulder_1',
T_DH(.10774, pi / 2, 0, -pi / 2), 'l_shoulder_2'],
'l_shoulder_yaw':
['l_shoulder_2', T_DH(0, -pi / 2, 0, pi / 2), 'l_arm'],
'l_elbow_pitch':
['l_arm',
T_DH(.15228, 75 * pi / 180, 0, -pi / 2), 'l_elbow_1'],
'l_elbow_yaw': ['l_elbow_1',
T_DH(0, 0, -.015, pi / 2), 'l_forearm'],
'l_wrist_roll':
['l_forearm',
T_DH(.1373, -pi / 2, 0, pi / 2), 'l_wrist_1'],
'l_wrist_pitch': ['l_wrist_1',
T_DH(0, pi / 2, 0, pi / 2), 'l_hand'],
'r_shoulder_pitch': [
'chest',
T_DH(-.1433, -105 * pi / 180, -.0233647, pi / 2), 'r_shoulder_1'
],
'r_shoulder_roll':
['r_shoulder_1',
T_DH(-.10774, -pi / 2, 0, pi / 2), 'r_shoulder_2'],
'r_shoulder_yaw':
['r_shoulder_2', T_DH(0, -pi / 2, 0, -pi / 2), 'r_arm'],
'r_elbow_pitch':
['r_arm',
T_DH(-.15228, -105 * pi / 180, 0, -pi / 2), 'r_elbow_1'],
'r_elbow_yaw': ['r_elbow_1',
T_DH(0, 0, .015, pi / 2), 'r_forearm'],
'r_wrist_roll':
['r_forearm',
T_DH(-.1373, -pi / 2, 0, pi / 2), 'r_wrist_1'],
'r_wrist_pitch': ['r_wrist_1',
T_DH(0, pi / 2, 0, pi / 2), 'r_hand'],
'l_hip_pitch':
['waist',
dot(transl(0, -.0681, -.1199),
rotx(-pi / 2)), 'l_hip_1'],
'l_hip_roll': ['l_hip_1',
T_DH(0, pi / 2, 0, -pi / 2), 'l_hip_2'],
'l_hip_yaw': ['l_hip_2',
T_DH(0, pi / 2, 0, -pi / 2), 'l_thigh'],
'l_knee': ['l_thigh',
T_DH(-.2236, -pi / 2, 0, pi / 2), 'l_shank'],
'l_ankle_pitch': ['l_shank',
T_DH(0, pi / 2, -.213, pi), 'l_ankle_1'],
'l_ankle_roll': ['l_ankle_1',
T_DH(0, 0, 0, -pi / 2), 'l_foot'],
'r_hip_pitch': [
'waist',
dot(transl(0, .0681, -.1199), rotx(-pi / 2)), 'r_hip_1'
],
'r_hip_roll': ['r_hip_1',
T_DH(0, pi / 2, 0, pi / 2), 'r_hip_2'],
'r_hip_yaw': ['r_hip_2',
T_DH(0, pi / 2, 0, pi / 2), 'r_thigh'],
'r_knee': ['r_thigh',
T_DH(.2236, -pi / 2, 0, -pi / 2), 'r_shank'],
'r_ankle_pitch': ['r_shank',
T_DH(0, pi / 2, -.213, pi), 'r_ankle_1'],
'r_ankle_roll': ['r_ankle_1',
T_DH(0, 0, 0, pi / 2), 'r_foot'],
}
def get_contact_data():
""" somes extras: give some other important frames
Each subframe is described as follows:
subframe name : [body frame, dims, H_body_subframe]
they can be used for contact point for example
"""
return {
'lh1' : ['l_hand', (.012,), transl(.012, .0205, 0.)],
'lh2' : ['l_hand', (.012,), transl(.012, -.0205, 0.)],
'lh3' : ['l_hand', (.012,), transl(.057, .0205, 0.)],
'lh4' : ['l_hand', (.012,), transl(.057, -.0205, 0.)],
'l_hand_tip' : ['l_hand', (.012,), transl(.069, 0, 0)],
'l_hand_palm' : ['l_hand', (.012,), transl(.0345, 0, 0)],
'rh1' : ['r_hand', (.012,), transl(-.012, .0205, 0.)],
'rh2' : ['r_hand', (.012,), transl(-.012, -.0205, 0.)],
'rh3' : ['r_hand', (.012,), transl(-.057, .0205, 0.)],
'rh4' : ['r_hand', (.012,), transl(-.057, -.0205, 0.)],
'r_hand_tip' : ['r_hand', (.012,), transl(-.069, 0, 0)],
'r_hand_palm' : ['r_hand', (.012,), transl(-.0345, 0, 0)],
'lf1' : ['l_foot', (.002,), transl(-.039,-.027,-.031)],
'lf2' : ['l_foot', (.002,), transl(-.039, .027,-.031)],
'lf3' : ['l_foot', (.002,), transl(-.039, .027, .099)],
'lf4' : ['l_foot', (.002,), transl(-.039,-.027, .099)],
'l_sole': ['l_foot', (), dot(transl(-.041, .0 , .034),
dot(roty(-pi/2), rotx(-pi/2) ) )],
'rf1' : ['r_foot', (.002,), transl(-.039,-.027, .031)],
'rf2' : ['r_foot', (.002,), transl(-.039, .027, .031)],
'rf3' : ['r_foot', (.002,), transl(-.039, .027,-.099)],
'rf4' : ['r_foot', (.002,), transl(-.039,-.027,-.099)],
'r_sole': ['r_foot', (), dot(transl(-.041, .0 ,-.034),
dot(roty(pi/2), rotx(pi/2) ) )],
}
error.append(sh-th)
return error
def plot_error(self):
plot(self.timeline, self.get_error())
def plot_height(self):
plot(self.timeline, self.height,
self.timeline, self.get_theoric())
legend(('simulated', 'theoric'))
w = ObservableWorld()
if True:
from arboris.homogeneousmatrix import transl
H_bc = transl(1,1,1)
else:
H_bc = eye(4)
lengths = (1.,1.,1.)
mass = 1.
body = Body(
name='box_body',
mass=massmatrix.transport(massmatrix.box(lengths, 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)
# body properties M = b.mass N = b.nleffects B = b.viscosity # information about its position in kinematic structure p_joint = b.parentjoint # return the parent joint c_joints = b.childrenjoints # return list of chidren joints ##### The SubFrame/MovingSubFrame classes ###################################### from arboris.core import SubFrame, MovingSubFrame from arboris.homogeneousmatrix import transl # How to create a fixed subframe H_b_f = transl(.1, .2, .3) # homogeneous matrix from body to subframe sf = SubFrame(body=w.ground, bpose=H_b_f, name="myFrame") # bpose constant # How to create a moving subframe #Warning: these are not designed to create the kinematic tree. # they are intended to be used as target for example msf = MovingSubFrame(w.ground, name="myMovingFrame") msf.bpose = H_b_f # possible only with MovingSubFrame instance ##### The Joint class ########################################################## # A joint relates a parent frame with a child frame by constraining their # relative motion. j = joints["Shoulder"] pose = j.pose # inverse method: ipose = rm.ipose()
def add_groundplane(w, half_extents=(1., .1, 1.) ):
"""Add a ground plane using a box shape.
"""
frame = SubFrame(w.ground, transl(0., -half_extents[1], 0.) )
w.register(Box(frame, half_extents, 'Ground shape'))
def add_groundplane(w, length=(1., .1, 1.) ):
"""Add a ground plane using a box shape.
"""
frame = SubFrame(w.ground, transl(0., -length[1]/2., 0.) )
w.register(Box(frame, length, 'Ground shape'))
def _human36(world, height=1.741, mass=73, name=''):
"""
TODO: HuMAnS' doc about inertia is erroneous (the real math is in the IOMatrix proc in DynamicData.maple)
"""
assert isinstance(world, World)
w = world
L = anatomical_lengths(height)
bodies = {}
humansbodyid_to_humansbodyname_map = {}
for b in _humans_bodies(height, mass):
#mass matrix at com
mass_g = b['Mass'] * diag(
hstack((b['GyrationRadius']**2, (1,1,1))))
H_fg = eye(4)
H_fg[0:3,3] = b['CenterOfMass']
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(adjoint(H_gf).T, dot(mass_g, adjoint(H_gf)))
bodies[b['HumansName']] = Body(
name=b['HumansName'],
mass=mass_o)
humansbodyid_to_humansbodyname_map[b['HumansId']] = b['HumansName']
rf = SubFrame(w.ground,
Hg.transl(0, L['yfootL']+L['ytibiaL']+L['yfemurL'], 0))
w.add_link(rf, FreeJoint(), bodies['LPT'])
rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, L['zhip']/2.))
j = RzRyRxJoint()
w.add_link(rf, j, bodies['ThighR'])
rf = SubFrame(bodies['ThighR'], Hg.transl(0, -L['yfemurR'], 0))
w.add_link(rf, RzJoint(), bodies['ShankR'])
rf = SubFrame(bodies['ShankR'], Hg.transl(0, -L['ytibiaR'], 0))
w.add_link(rf, RzRxJoint(), bodies['FootR'])
rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, -L['zhip']/2.))
w.add_link(rf, RzRyRxJoint(), bodies['ThighL'])
rf = SubFrame(bodies['ThighL'], Hg.transl(0, -L['yfemurL'], 0))
w.add_link(rf, RzJoint(), bodies['ShankL'])
rf = SubFrame(bodies['ShankL'], Hg.transl(0, -L['ytibiaL'], 0))
w.add_link(rf, RzRxJoint(), bodies['FootL'])
rf = SubFrame(bodies['LPT'], Hg.transl(-L['xvT10'], L['yvT10'], 0))
w.add_link(rf, RzRyRxJoint(), bodies['UPT'])
rf = SubFrame(bodies['UPT'],
Hg.transl(L['xsternoclavR'], L['ysternoclavR'], L['zsternoclavR']))
j = RyRxJoint()
w.add_link(rf, j, bodies['ScapulaR'])
rf = SubFrame(bodies['ScapulaR'],
Hg.transl(-L['xshoulderR'], L['yshoulderR'], L['zshoulderR']))
w.add_link(rf, RzRyRxJoint(), bodies['ArmR'])
rf = SubFrame(bodies['ArmR'], Hg.transl(0, -L['yhumerusR'], 0))
w.add_link(rf, RzRyJoint(), bodies['ForearmR'])
rf = SubFrame(bodies['ForearmR'], Hg.transl(0, -L['yforearmR'], 0))
w.add_link(rf, RzRxJoint(), bodies['HandR'])
rf = SubFrame(bodies['UPT'], Hg.transl(
L['xsternoclavL'], L['ysternoclavL'], -L['zsternoclavL']))
w.add_link(rf, RyRxJoint(), bodies['ScapulaL'])
rf = SubFrame(bodies['ScapulaL'],
Hg.transl(-L['xshoulderL'], L['yshoulderL'], -L['zshoulderL']))
w.add_link(rf, RzRyRxJoint(), bodies['ArmL'])
rf = SubFrame(bodies['ArmL'], Hg.transl(0, -L['yhumerusL'], 0))
w.add_link(rf, RzRyJoint(), bodies['ForearmL'])
rf = SubFrame(bodies['ForearmL'], Hg.transl(0, -L['yforearmL'], 0))
w.add_link(rf, RzRxJoint(), bodies['HandL'])
rf = SubFrame(bodies['UPT'], Hg.transl(L['xvT10'], L['yvC7'], 0))
w.add_link(rf, RzRyRxJoint(), bodies['Head'])
# add tags
tags = {}
for t in _humans_tags(height):
bodyname = humansbodyid_to_humansbodyname_map[t['HumansBodyId']]
tag = SubFrame(
bodies[bodyname],
Hg.transl(t['Position'][0], t['Position'][1], t['Position'][2]),
t['HumansName'])
tags[t['HumansName']] = tag
w.register(tag)
# Add point shapes to the feet
for k in ('Right foot toe tip', 'Right foot heel',
'Right foot phalange 5', 'Right foot Phalange 1',
'Left foot toe tip','Left foot heel',
'Left foot phalange 5','Left foot phalange 1'):
shape = Point(tags[k], name=k)
w.register(shape)
w.init()
return (bodies, tags)
from arboris.core import SubFrame from arboris.shapes import Sphere, Cylinder from arboris.homogeneousmatrix import transl from arboris.constraints import SoftFingerContact ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(.5,.5,.5)) frames = w.getframes() w.register(Sphere(frames['EndEffector'],radius=.02, name='ee') ) w.register(Sphere(SubFrame(w.ground, transl(-.8, .2, 0.), name='obstacle'),radius=.1, name='obstacle') ) w.register(Sphere(SubFrame(w.ground, transl(-.55, .2, 0.), name='obstacle2'),radius=.1, name='obstacle2') ) shapes = w.getshapes() #w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01)) ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import FrameTask
vel = self.parentjoint.gvel
force = self.kp*(posdes-pos) - self.kd*vel
gforce[self.parentjoint.dof] = force
gforce[self.parentjoint.dof] += self.mass*9.81
return gforce, impedance
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
add_table(w, hl=(.05,.2,.02), init_pos=transl(-.3,0,.6), mass=10., name='wall')
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
shapes = w.getshapes()
consts = w.getconstraints()
##POS of the arms, before or after the rec of standing_pos?
joints['l_shoulder_pitch'].gpos[:] = -0.65695599
joints['l_shoulder_roll'].gpos[:] = 0.58153898
joints['l_shoulder_yaw'].gpos[:] = 0.68249881
joints['l_elbow_pitch'].gpos[:] = 1.27058836
joints['l_elbow_yaw'].gpos[:] = 0.85522174
joints['l_wrist_roll'].gpos[:] = 0.15702068
joints['l_wrist_pitch'].gpos[:] = 0.41882797
def pose(self):
return Hg.transl(0., self.gpos[0], 0.)
const = w.getconstraints()
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import ForceTask, FrameTask
from LQPctrl.task_ctrl import ValueCtrl, KpCtrl
from arboris.homogeneousmatrix import rotz, transl
from numpy import pi
tasks = []
goal = rotz(pi / 8)
goal[0:3, 3] = [-0.4, .5, 0]
goal = transl(-.8, -.5, 0)
tasks.append(
ForceTask(const["const"], ValueCtrl([0, 0.001, .01]), [], 1., 0, True))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2}
lqpc = LQPcontroller(gforcemax, tasks=tasks)
w.register(lqpc)
############################
# #
Matrices and vectors are representeed with numpy arrays.
Here, we present some functions and operations to do mechanics.
"""
import numpy as np
np.set_printoptions(suppress=True)
##### About homogeneous matrix #################################################
import arboris.homogeneousmatrix as Hg
H_o_a = Hg.transl(1,2,3) # get a translation homogeneous matrix
H_a_b = Hg.rotzyx(.1,.2,.3) # get a rotation R = Rz * Ry * Rx
# also Hg.rotzy R = Rz * Ry
# Hg.rotzx R = Rz * Rx
# Hg.rotyx R = Ry * Rx
# Hg.rotz R = Rz
# Hg.roty R = Ry
# Hg.rotx R = Rx
H_o_b = np.dot(H_o_a, H_a_b) # H from {o} to {b} is H{o}->{a} * H{a}->{b}
H_b_o = Hg.inv(H_o_b) # obtain inverse, H from {b} to {o}
print("H_o_b * H_b_o\n", np.dot(H_o_b, H_b_o))
def add_link(body0, transl, joint, body1):
if not isinstance(body0, Body):
body0 = bodies[prefix + body0]
frame0 = SubFrame(body0, Hg.transl(*transl))
w.add_link(frame0, joint, bodies[prefix + body1])
error.append(sh - th)
return error
def plot_error(self):
plot(self.timeline, self.get_error())
def plot_height(self):
plot(self.timeline, self.height, self.timeline, self.get_theoric())
legend(('simulated', 'theoric'))
w = World()
if True:
from arboris.homogeneousmatrix import transl
H_bc = transl(1, 1, 1)
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)
def pose(self):
return Hg.transl(self.gpos[0], self.gpos[1], self.gpos[2])
def get_bodies_data():
""" This dictionnary describes the bodies of the robot
Each body is a list of shape
name : [ [(dims1) | mass1 | H1],
[(dims2) | mass2 | H2] ]
about dims (in meter):
if dims has 1 element: it is a sphere; get (radius,)
if dims has 2 elements: it is a cylinder with z-axis; get (length, radius)
if dims has 3 elements: it is a box; get half-lengths along (x,y,z)
masses are in kg
about transformation matrix H:
it represents the transformation from the body frame to the center of
the shape.
transl for translation along (x,y,z) (meter)
rotx,roty, rotz for rotation around x,y,z axis
dot for matrices multiplication
"""
return {
### WARNING: pb between shape and inertia in the c++ file of the simulator
#'waist' : [[(0.002, 0.065, 0.027), 0.20297, dot(transl(0, 0, -0.065), rotx(pi/2))]],
'waist': [[(.032, .0235, .057215), .20297,
transl(.006, 0, -.1)]],
'lap_belt_1': [[(.0315, .0635, .088), 3.623,
dot(transl(-.0383, -.0173, 0), rotz(-pi / 4))]],
'lap_belt_2': [[(.097, .031), .91179,
transl(0, 0, .008)]],
'chest': [[(.0274, .04), .45165,
transl(0, 0, -.0447)],
[(.036, .0545, .059), 1.8388,
dot(transl(-.03, 0, -.1174), rotz(pi / 12))],
[(.036, .0545, .059), 1.8388,
dot(transl(.03, 0, -.1174), rotz(-pi / 12))]],
'neck_1': [[(.077, .015), .28252, eye(4)]],
'neck_2': [[(.033, .015), .1, eye(4)]],
'head': [[(.08, ), .78, transl(0, 0, .0825)]],
'l_shoulder_1': [[(.011, .031), .48278,
transl(0, 0, .07224)]],
'l_shoulder_2': [[(.059, .03), .20779, eye(4)]],
'l_arm': [[(.156, .026), 1.1584,
transl(0, 0, .078)]],
'l_elbow_1': [[(.01, ), .050798, eye(4)]],
'l_forearm': [[(.14, .02), .48774,
transl(0, 0, .07)]],
'l_wrist_1': [[(.04, .01), .05, eye(4)]],
'l_hand': [[(.0345, .0325, .012), .19099,
transl(.0345, 0, 0)]],
'r_shoulder_1': [[(.011, .031), .48278,
transl(0, 0, -.07224)]],
'r_shoulder_2': [[(.059, .03), .20779, eye(4)]],
'r_arm': [[(.156, .026), 1.1584,
transl(0, 0, -.078)]],
'r_elbow_1': [[(.01, ), .050798, eye(4)]],
'r_forearm': [[(.14, .02), .48774,
transl(0, 0, -.07)]],
'r_wrist_1': [[(.04, .01), .05, eye(4)]],
'r_hand': [[(.0345, .0325, .012), .19099,
transl(-.0345, 0, 0)]],
'l_hip_1': [[(.013, .038), .32708,
transl(0, 0, .0375)]],
'l_hip_2': [[(.075, .031), 1.5304, eye(4)]],
'l_thigh': [[(.224, .034), 1.5304,
transl(0, 0, -.112)],
[(.077, .0315), .79206,
dot(transl(0, 0, -.224), roty(pi / 2))]],
'l_shank': [[(.213, .0315), .95262,
dot(transl(0, -.1065, 0), rotx(pi / 2))]],
'l_ankle_1': [[(.063, .0245), .14801, eye(4)]],
'l_foot': [[(.095, .027), .59285,
transl(0, 0, .0125)],
[(.002, .027, .065), .08185,
transl(-.039, 0, .034)]],
'r_hip_1': [[(.013, .038), .32708,
transl(0, 0, -.0375)]],
'r_hip_2': [[(.075, .031), 1.5304, eye(4)]],
'r_thigh': [[(.224, .034), 1.5304,
transl(0, 0, .112)],
[(.077, .0315), .79206,
dot(transl(0, 0, .224), roty(pi / 2))]],
'r_shank': [[(.213, .0315), .95262,
dot(transl(0, -.1065, 0), rotx(pi / 2))]],
'r_ankle_1': [[(.063, .0245), .14801, eye(4)]],
'r_foot': [[(.095, .027), .59285,
transl(0, 0, -.0125)],
[(.002, .027, .065), .08185,
transl(-.039, 0, -.034)]],
}
def ipose(self):
return Hg.transl(-self.gpos[0], 0., 0.)
def get_contact_data():
""" somes extras: give some other important frames
Each subframe is described as follows:
subframe name : [body frame, dims, H_body_subframe]
they can be used for contact point for example
"""
return {
'lh1': ['l_hand', (.012, ),
transl(.012, .0205, 0.)],
'lh2': ['l_hand', (.012, ),
transl(.012, -.0205, 0.)],
'lh3': ['l_hand', (.012, ),
transl(.057, .0205, 0.)],
'lh4': ['l_hand', (.012, ),
transl(.057, -.0205, 0.)],
'l_hand_tip': ['l_hand', (.012, ),
transl(.069, 0, 0)],
'l_hand_palm': ['l_hand', (.012, ),
transl(.0345, 0, 0)],
'rh1': ['r_hand', (.012, ),
transl(-.012, .0205, 0.)],
'rh2': ['r_hand', (.012, ),
transl(-.012, -.0205, 0.)],
'rh3': ['r_hand', (.012, ),
transl(-.057, .0205, 0.)],
'rh4': ['r_hand', (.012, ),
transl(-.057, -.0205, 0.)],
'r_hand_tip': ['r_hand', (.012, ),
transl(-.069, 0, 0)],
'r_hand_palm': ['r_hand', (.012, ),
transl(-.0345, 0, 0)],
'lf1': ['l_foot', (.002, ),
transl(-.039, -.027, -.031)],
'lf2': ['l_foot', (.002, ),
transl(-.039, .027, -.031)],
'lf3': ['l_foot', (.002, ),
transl(-.039, .027, .099)],
'lf4': ['l_foot', (.002, ),
transl(-.039, -.027, .099)],
'l_sole': [
'l_foot', (),
dot(transl(-.041, .0, .034), dot(roty(-pi / 2), rotx(-pi / 2)))
],
'rf1': ['r_foot', (.002, ),
transl(-.039, -.027, .031)],
'rf2': ['r_foot', (.002, ),
transl(-.039, .027, .031)],
'rf3': ['r_foot', (.002, ),
transl(-.039, .027, -.099)],
'rf4': ['r_foot', (.002, ),
transl(-.039, -.027, -.099)],
'r_sole': [
'r_foot', (),
dot(transl(-.041, .0, -.034), dot(roty(pi / 2), rotx(pi / 2)))
],
}
def pose(self):
return Hg.transl(0., self.gpos[0], 0.)
self.f = f
def init(self, world, timeline):
pass
def update(self, dt):
print "F.pose: ", self.f.pose
def finish(self):
pass
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
w.register(Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.5), name='obstacle'),radius=.05, name='obstacle') )
w.register(Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.63), name='obstacle'),radius=.05, name='obstacle2') )
#w.register(Cylinder(SubFrame(w.ground, transl(-.5, 0, 0.7), name='obstacle'),radius=.1, length=.2, name='obstacle') )
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
shapes = w.getshapes()
consts = w.getconstraints()
icub_joints = [joints[n] for n in icub.get_joints_data()]
standing_pose = [j.gpos[0] for j in icub_joints]
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
def add_simplearm(world, name='', lengths=(0.5 ,0.4 , 0.2),
masses=(1.0, 0.8, 0.2)):
"""Build a planar 3-R robot.
TODO: make use of the ``name`` input argument to prefix bodies and joints names
Example:
>>> w = World()
>>> add_simplearm(w)
>>> w.update_dynamic()
"""
assert isinstance(world, World)
w = world
arm_length = lengths[0]
arm_mass = masses[0]
forearm_length = lengths[1]
forearm_mass = masses[1]
hand_length = lengths[2]
hand_mass = masses[2]
# create bodies
arm = Body(
name=name+'Arm',
mass=transport_mass_matrix(
mass_parallelepiped(
arm_mass,
(arm_length/10,arm_length,arm_length/10)),
Hg.transl(0,arm_length/2,0)))
forearm = Body(
name=name+'ForeArm',
mass=transport_mass_matrix(
mass_parallelepiped(
forearm_mass,
(forearm_length/10,forearm_length,forearm_length/10)),
Hg.transl(0,forearm_length/2,0)))
hand = Body(
name=name+'Hand',
mass=transport_mass_matrix(
mass_parallelepiped(
hand_mass,
(hand_length/10,hand_length,hand_length/10)),
Hg.transl(0,hand_length/2,0)))
# create a joint between the ground and the arm
shoulder = RzJoint(name=name+'Shoulder')
w.add_link(w.ground, shoulder, arm)
# add a frame to the arm, where the forearm will be anchored
f = SubFrame(arm,
Hg.transl(0,arm_length,0),
name+'ElbowBaseFrame')
# create a joint between the arm and the forearm
elbow = RzJoint(name=name+'Elbow')
w.add_link(f, elbow, forearm)
# add a frame to the forearm, where the hand will be anchored
f = SubFrame(forearm,
Hg.transl(0,forearm_length,0),
name+'WristBaseFrame')
# create a joint between the forearm and the hand
wrist = RzJoint(name=name+'Wrist')
w.add_link(f, wrist, hand)
# create a frame at the end of the hand
f = SubFrame(hand, Hg.transl(0,hand_length,0), name+'EndEffector')
w.register(f)
w.init()
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import ForceTask, FrameTask
from LQPctrl.task_ctrl import ValueCtrl, KpCtrl
from arboris.homogeneousmatrix import rotz, transl
from numpy import pi
tasks = []
goal = rotz(pi/8)
goal[0:3,3] = [-0.4, .5, 0]
goal = transl(-.8,-.5,0)
tasks.append(ForceTask( const["const"], ValueCtrl([0, 0.001,.01]) , [], 1., 0, True))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder":10,"Elbow":5,"Wrist":2}
lqpc = LQPcontroller(gforcemax, tasks=tasks)
w.register(lqpc)
def add_link(body0, transl, joint, body1):
if not isinstance(body0, Body):
body0 = bodies[prefix+body0]
frame0 = SubFrame(body0, Hg.transl(*transl))
w.add_link(frame0, joint, bodies[prefix+body1])
def update(self, dt):
print "F.pose: ", self.f.pose
def finish(self):
pass
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
w.register(
Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.5), name='obstacle'),
radius=.05,
name='obstacle'))
w.register(
Sphere(SubFrame(w.ground, transl(-.16, -0.1, 0.63), name='obstacle'),
radius=.05,
name='obstacle2'))
#w.register(Cylinder(SubFrame(w.ground, transl(-.5, 0, 0.7), name='obstacle'),radius=.1, length=.2, name='obstacle') )
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
shapes = w.getshapes()
consts = w.getconstraints()
icub_joints = [joints[n] for n in icub.get_joints_data()]
def pose(self):
return Hg.transl(self.gpos[0], self.gpos[1], self.gpos[2])
from arboris.controllers import WeightController
w.register(WeightController())
##### Add observers
from arboris.observers import PerfMonitor, Hdf5Logger
from arboris.visu.dae_writer import write_collada_scene, write_collada_animation, add_shapes_to_dae
from arboris.visu import pydaenimCom
flat = False
write_collada_scene(w, "./scene.dae", flat=flat)
shapes_info = [
["Arm" , "./dae/icub_simple.dae#head"], # parent frame, child shape node id
["Forearm" , "./dae/icub_simple.dae#head", transl(0,0.3,0)], # parent frame, child shape node id, H_frame_shape
["EndEffector", "./dae/icub_simple.dae" , transl(0,0.3,0), (0.3,0.3,0.3)], # parent frame, child shape file , H_frame_shape, shape_scale
]
add_shapes_to_dae("./scene.dae", shapes_info) # add argument output_file="out.dae" if you want to keep original dae file
obs = []
pobs = PerfMonitor(True)
dobs = pydaenimCom("./scene.dae", flat=flat)
h5obs = Hdf5Logger("sim.h5", mode="w", flat=flat)
obs.append(pobs)
obs.append(dobs)
obs.append(h5obs)
def ipose(self):
return Hg.transl(-self.gpos[0], 0., 0.)
force = self.kp * (posdes - pos) - self.kd * vel
gforce[self.parentjoint.dof] = force
gforce[self.parentjoint.dof] += self.mass * 9.81
return gforce, impedance
#################################
# #
# CREATE WORLD & INITIALIZATION #
# #
#################################
w = create_icub_and_init(gravity=True)
add_table(w,
hl=(.05, .2, .02),
init_pos=transl(-.3, 0, .6),
mass=10.,
name='wall')
joints = w.getjoints()
frames = w.getframes()
bodies = w.getbodies()
shapes = w.getshapes()
consts = w.getconstraints()
##POS of the arms, before or after the rec of standing_pos?
joints['l_shoulder_pitch'].gpos[:] = -0.65695599
joints['l_shoulder_roll'].gpos[:] = 0.58153898
joints['l_shoulder_yaw'].gpos[:] = 0.68249881
joints['l_elbow_pitch'].gpos[:] = 1.27058836
joints['l_elbow_yaw'].gpos[:] = 0.85522174
def get_meshes_positions():
return {
"head_mesh" : dot(transl(0,0,-0.055), rotzyx(pi/2., 0, 0)),
"torso_mesh" : dot(transl(0,0.024,-0.144), rotzyx(-pi/2., pi, 0)),
"pelvis_mesh" : dot(transl(0.031,0.04,0), rotzyx(pi/2.,pi,pi/2.)),
"l_upperleg_mesh": dot(transl(0,0,-.05), rotzyx( pi/2., 0, 0)),
"r_upperleg_mesh": dot(transl(0,0, .05), rotzyx(-pi/2., pi, 0)),
"l_lowerleg_mesh": rotzyx(pi,0, pi/2.),
"r_lowerleg_mesh": rotzyx(pi,0, pi/2.),
"l_foot_mesh" : dot(transl(-0.041,0,-0.03), rotzyx(0, pi/2.,0)),
"r_foot_mesh" : dot(transl(-0.041,0, 0.03), rotzyx(pi,-pi/2.,0)),
"l_upperarm_mesh": dot(transl(0,0.01, 0.14), rotzyx(-pi/2.,0,-pi/2.)),
"r_upperarm_mesh": dot(transl(0,0.01,-0.14), rotzyx(-pi/2.,0,-pi/2.)),
"l_forearm_mesh" : dot(transl(0,0,-0.004), rotzyx(pi,0,-pi/2.)),
"r_forearm_mesh" : dot(transl(0,0, 0.004), rotzyx(0,0,-pi/2.)),
## simple meshes
"waist_mesh" : transl(.006, 0,-.1),
"chest_cylinder_mesh": transl(0, 0, -.0447),
"neck_1_mesh" : eye(4),
"neck_2_mesh" : eye(4),
"head_sphere_mesh": transl(0, 0, .0825),
"l_shoulder_1_mesh": transl(0, 0, .07224),
"l_shoulder_2_mesh": eye(4),
"l_arm_cyl_mesh": transl(0, 0, .078),
"l_elbow_1_mesh": eye(4),
"l_forearm_cyl_mesh": transl(0, 0, .07),
"l_wrist_1_mesh": eye(4),
"l_hand_mesh": transl(.0345, 0, 0),
"r_shoulder_1_mesh": transl(0, 0, -.07224),
"r_shoulder_2_mesh": eye(4),
"r_arm_cyl_mesh": transl(0, 0, -.078),
"r_elbow_1_mesh": eye(4),
"r_forearm_cyl_mesh":transl(0, 0, -.07),
"r_wrist_1_mesh": eye(4),
"r_hand_mesh": transl(-.0345, 0, 0),
"l_hip_1_mesh": transl(0, 0, .0375),
"l_hip_2_mesh": eye(4),
"l_thigh_cyl1_mesh": transl(0, 0, -.112),
"l_thigh_cyl2_mesh": dot(transl(0, 0, -.224), roty(pi/2)),
"l_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)),
"l_ankle_1_mesh": eye(4),
"l_foot_cyl_mesh": transl(0,0,.0125),
"r_hip_1_mesh": transl(0, 0, -.0375),
"r_hip_2_mesh": eye(4),
"r_thigh_cyl1_mesh": transl(0, 0, .112),
"r_thigh_cyl2_mesh": dot(transl(0, 0, .224), roty(pi/2)),
"r_shank_cyl_mesh": dot(transl(0, -.1065, 0), rotx(pi/2)),
"r_ankle_1_mesh": eye(4),
"r_foot_cyl_mesh": transl(0,0,-.0125),
}
