def add_dummy_collision(list):
"""Add a Box collision tag to every links containing keyword in list."""
global robot
for string in list:
for link in robot.links:
if robot.links[link].name.find(string) != -1:
robot.links[link].collision = ur.Collision(
ur.Box([0.01, 0.01, 0.005]),
ur.Pose((0, 0, 0), (0, 0, 0)))
try:
robot.joints[joint].child = LINKS_DICO[robot.joints[joint].child]
except KeyError:
pass
for link in robot.links.keys():
try:
robot.rename_link(link, LINKS_DICO[link])
except KeyError, ValueError:
pass
if NAME == 'romeo':
robot.add_link(ur.Link('gaze'))
robot.add_joint(
ur.Joint(
'gaze_joint', 'HeadRoll_link', 'gaze', 'fixed', None,
ur.Pose((OFFSETS_DICO['CameraLeftEyeOffsetX'], 0,
OFFSETS_DICO['CameraLeftEyeOffsetZ']), (0, 0, 0))))
MESH_VERSION = ""
elif NAME == "nao":
robot.add_link(ur.Link('gaze'))
robot.add_joint(
ur.Joint(
'gaze_joint', 'Head', 'gaze', 'fixed', None,
ur.Pose((OFFSETS_DICO['CameraTopV4OffsetX'], 0,
OFFSETS_DICO['CameraTopV4OffsetZ']), (0, 0, 0))))
if VERSION == 'V32':
MESH_VERSION = VERSION
elif VERSION == 'V33' or VERSION == 'V40' or VERSION == 'V50':
MESH_VERSION = 'V40'
elif NAME == "pepper":
pass
try:
robot.joints[joint].child = LINKS_DICO[robot.joints[joint].child]
except KeyError:
pass
for link in robot.links.keys():
try:
robot.rename_link(link, LINKS_DICO[link])
except KeyError, ValueError:
pass
if NAME == 'romeo':
robot.add_link(ur.Link('gaze'))
robot.add_joint(ur.Joint('gaze_joint', 'HeadRoll_link',
'gaze', 'fixed', None, ur.Pose(
(OFFSETS_DICO['CameraLeftEyeOffsetX'], 0,
OFFSETS_DICO['CameraLeftEyeOffsetZ']), (0, 0, 0))))
MESH_VERSION = ''
elif NAME == 'nao':
robot.add_link(ur.Link('gaze'))
robot.add_joint(ur.Joint('gaze_joint', 'Head',
'gaze', 'fixed', None, ur.Pose(
(OFFSETS_DICO['CameraTopV4OffsetX'], 0,
OFFSETS_DICO['CameraTopV4OffsetZ']), (0, 0, 0))))
if VERSION == 'V32':
MESH_VERSION = VERSION
elif VERSION == 'V33' or VERSION == 'V40' or VERSION == 'V50':
MESH_VERSION = 'V40'
elif NAME == 'pepper':
u'LShoulderRoll_link': ur.Cylinder(0.015, 0.09),
u'LElbowRoll_link': ur.Cylinder(0.015, 0.05065),
u'LHipPitch_link': ur.Cylinder(0.015, 0.1),
u'LKneePitch_link': ur.Cylinder(0.015, 0.1),
u'RHipPitch_link': ur.Cylinder(0.015, 0.1),
u'RKneePitch_link': ur.Cylinder(0.015, 0.1),
u'RShoulderRoll_link': ur.Cylinder(0.015, 0.09),
u'RElbowRoll_link': ur.Cylinder(0.015, 0.05065),
u'LAnkleRoll_link': ur.Box((0.16, 0.06, 0.015)),
u'RAnkleRoll_link': ur.Box((0.16, 0.06, 0.015)),
u'LWristYaw_link': ur.Cylinder(0.015, 0.058),
u'RWristYaw_link': ur.Cylinder(0.015, 0.058),
}
Nao_orig = {
u'Torso_link': ur.Pose((0, 0, 0.02075), (0, 0, 0)),
u'HeadPitch_link': ur.Pose((0, 0, 0.058), (pi_2, 0, 0)),
u'LShoulderRoll_link': ur.Pose((0.045, 0, 0), (pi_2, 0, pi_2)),
u'LElbowRoll_link': ur.Pose((0.025325, 0, 0), (pi_2, 0, pi_2)),
u'LHipPitch_link': ur.Pose((0, 0, -0.05), (0, 0, 0)),
u'LKneePitch_link': ur.Pose((0, 0, -0.05), (0, 0, 0)),
u'RHipPitch_link': ur.Pose((0, 0, -0.05), (0, 0, 0)),
u'RKneePitch_link': ur.Pose((0, 0, -0.05), (0, 0, 0)),
u'RShoulderRoll_link': ur.Pose((0.045, 0, 0), (pi_2, 0, pi_2)),
u'RElbowRoll_link': ur.Pose((0.025325, 0, 0), (pi_2, 0, pi_2)),
u'LAnkleRoll_link': ur.Pose((0.02, 0, 0.0075), (0, 0, 0)),
u'RAnkleRoll_link': ur.Pose((0.02, 0, 0.0075), (0, 0, 0)),
u'LWristYaw_link': ur.Pose((0.029, 0, 0), (pi_2, 0, pi_2)),
u'RWristYaw_link': ur.Pose((0.029, 0, 0), (pi_2, 0, pi_2)),
}