def __init__(self,
armLinkDefinitions,
armName="arm1"):
'''
Description:constructor for the robot arm
param:armLinkDefinitions:dictionary defining the links'
param:armLinkDefinitions:type:dictionary (see robot-def-??.json)
'''
self.lock = threading.Lock()
self.__links = []
self.__motionThreads = []
self.killThread = False
for _, link in sorted(armLinkDefinitions[armName]["links"].items()):
if "virtual" not in link:
# Retrieve the dependent link if there is one
if "gyroLinkDep" in link:
depLink = [curLink for curLink in self.__links if curLink.linkID == link["gyroLinkDep"]][0]
else:
depLink = None
self.__links.append(ArmLink(motorAddress=link["output"],
jointScrewB=link["jointScrewB"],
angleLimit=radians(link["angleLimit"]),
linkID=link["linkID"],
motorMinSpeedDPS=link["motorMinSpeedDPS"],
motorMaxSpeedDPS=link["motorMaxSpeedDPS"],
maxDelta=link["maxDelta"],
gearRatio=link["gearRatio"],
initOrder=link["initOrder"],
gyroSensor=link["gyroSensor"],
gyroAngleVerticalOffsetX=link["gyroAngleVerticalOffsetX"] if "gyroAngleVerticalOffsetX" in link else None,
gyroLinkDep=depLink,
polarity=link["polarity"],
wiggleDeg=link["wiggleDeg"]))
else:
# Modeled links (to compensate the lack of IK for the mobile manipulator)
self.__links.append(ArmLink(jointScrewB=link["jointScrewB"],
angleLimit=radians(link["angleLimit"]),
linkID=link["linkID"],
initOrder=link["initOrder"],
virtualLink=link["virtual"]))
# region EndEffector Init
if armLinkDefinitions[armName]["endEffector"]:
endEffectorDefinition = armLinkDefinitions[armName]["endEffector"]
self.__endEffector = EndEffector(motor_address=endEffectorDefinition["output"],
status=EndEffector.OPENED_GRIP,
initSpeedPercent=endEffectorDefinition["initSpeedPercent"],
initMethod=endEffectorDefinition["initMethod"],
openClose=endEffectorDefinition["openClose"])
self.__endEffector.initialize(percentSpeedTest=self.__endEffector.initSpeedPercent,
method=self.__endEffector.initMethod)
else:
self.__endEffector = None
self.armZeroPosition = radians(armLinkDefinitions[armName]["armZeroPosition"])
self.armRestPosition = radians(armLinkDefinitions[armName]["armRestPosition"])
self.armDrivePosition = radians(armLinkDefinitions[armName]["armDrivePosition"])
self.__links.sort(key=lambda x: x.linkID)
log.info(LOGGER_ARM_MAIN, 'Done arm initialization.')
def __init__(self, sim, manipulator):
EndEffector.__init__(self, manipulator)
robot = manipulator.GetRobot()
env = robot.GetEnv()
self.simulated = sim
with env:
accel_limits = robot.GetDOFAccelerationLimits()
accel_limits[self.GetIndices()] = 1.
robot.SetDOFAccelerationLimits(accel_limits)
if sim:
robot = manipulator.GetRobot()
self.controller = robot.AttachController(
self.GetName(), '', self.GetIndices(), 0, True)
def __init__(self, sim, manipulator, owd_namespace, bhd_namespace, ft_sim=True):
"""End-effector wrapper for the BarrettHand.
This class wraps a BarrettHand end-effector that is controlled by BHD
or OWD. The or_owd_controller, or_handstate_sensor, and
or_barrett_ft_sensor packages are used to communicate with the
underlying hardware drivers through ROS. Note that some methods (e.g.
reading breakaway status) are not supported on the BH-262.
@param sim whether the hand is simulated
@param manipulator manipulator the hand is attached to
@param owd_namespace ROS namespace that OWD is running in
@param bhd_namespace ROS namespace that the BarrettHand driver is running in
@param ft_sim whether the force/torque sensor is simulated
"""
EndEffector.__init__(self, manipulator)
self.simulated = sim
# Set the closing direction of the hand. This is only really necessary
# for the programmatically loaded HERB model. We can't easily specify
# ClosingDirection in URDF or SRDF.
gripper_indices = manipulator.GetGripperIndices()
closing_direction = numpy.zeros(len(gripper_indices))
finger_indices = self.GetFingerIndices()
for i, dof_index in enumerate(gripper_indices):
if dof_index in finger_indices:
closing_direction[i] = 1.
manipulator.SetChuckingDirection(closing_direction)
# Load the hand controller.
robot = self.manipulator.GetRobot()
env = robot.GetEnv()
self.controller = robot.AttachController(name=self.GetName(),
args='BHController {0:s} {1:s}'.format('prpy', bhd_namespace),
dof_indices=self.GetIndices(), affine_dofs=0, simulated=sim)
# Hand state, force/torque sensor, and tactile pads.
if not sim:
self.handstate_sensor = util.create_sensor(env,
'HandstateSensor {0:s} {1:s}'.format('prpy', bhd_namespace))
self.ft_simulated = ft_sim
if not ft_sim:
self.ft_sensor = util.create_sensor(env,
'BarrettFTSensor {0:s} {1:s}'.format('prpy', owd_namespace))
class Arm(object):
"""
Implementation of an arm (that contains a collection of links)
"""
__slots__ = [
'__links',
'stopAction',
'lock',
'_urgentStop',
'_killThread',
'__endEffector',
'__motionThreads',
'armZeroPosition',
'armRestPosition',
'armDrivePosition'
]
def __init__(self,
armLinkDefinitions,
armName="arm1"):
'''
Description:constructor for the robot arm
param:armLinkDefinitions:dictionary defining the links'
param:armLinkDefinitions:type:dictionary (see robot-def-??.json)
'''
self.lock = threading.Lock()
self.__links = []
self.__motionThreads = []
self.killThread = False
for _, link in sorted(armLinkDefinitions[armName]["links"].items()):
if "virtual" not in link:
# Retrieve the dependent link if there is one
if "gyroLinkDep" in link:
depLink = [curLink for curLink in self.__links if curLink.linkID == link["gyroLinkDep"]][0]
else:
depLink = None
self.__links.append(ArmLink(motorAddress=link["output"],
jointScrewB=link["jointScrewB"],
angleLimit=radians(link["angleLimit"]),
linkID=link["linkID"],
motorMinSpeedDPS=link["motorMinSpeedDPS"],
motorMaxSpeedDPS=link["motorMaxSpeedDPS"],
maxDelta=link["maxDelta"],
gearRatio=link["gearRatio"],
initOrder=link["initOrder"],
gyroSensor=link["gyroSensor"],
gyroAngleVerticalOffsetX=link["gyroAngleVerticalOffsetX"] if "gyroAngleVerticalOffsetX" in link else None,
gyroLinkDep=depLink,
polarity=link["polarity"],
wiggleDeg=link["wiggleDeg"]))
else:
# Modeled links (to compensate the lack of IK for the mobile manipulator)
self.__links.append(ArmLink(jointScrewB=link["jointScrewB"],
angleLimit=radians(link["angleLimit"]),
linkID=link["linkID"],
initOrder=link["initOrder"],
virtualLink=link["virtual"]))
# region EndEffector Init
if armLinkDefinitions[armName]["endEffector"]:
endEffectorDefinition = armLinkDefinitions[armName]["endEffector"]
self.__endEffector = EndEffector(motor_address=endEffectorDefinition["output"],
status=EndEffector.OPENED_GRIP,
initSpeedPercent=endEffectorDefinition["initSpeedPercent"],
initMethod=endEffectorDefinition["initMethod"],
openClose=endEffectorDefinition["openClose"])
self.__endEffector.initialize(percentSpeedTest=self.__endEffector.initSpeedPercent,
method=self.__endEffector.initMethod)
else:
self.__endEffector = None
self.armZeroPosition = radians(armLinkDefinitions[armName]["armZeroPosition"])
self.armRestPosition = radians(armLinkDefinitions[armName]["armRestPosition"])
self.armDrivePosition = radians(armLinkDefinitions[armName]["armDrivePosition"])
self.__links.sort(key=lambda x: x.linkID)
log.info(LOGGER_ARM_MAIN, 'Done arm initialization.')
# region properties
@property
def bList(self):
return np.array([link.jointScrewB for link in self.__links]).T
@property
def odometryPosition(self):
curThetaString = ''
for link in [link for link in self.__links if link.virtual is False]:
curThetaString += 'j{0}:{1:.1f} - '.format(link.linkID, link.armAngleDegX)
return curThetaString[:-2]
@property
def urgentStop(self):
return self._urgentStop
@urgentStop.setter
def urgentStop(self, value):
assert isinstance(value, bool), "UrgentStop should be a bool, it's of type {}".format(type(value))
for link in [link for link in self.__links if link.initOrder > 0]:
link.urgentStop = value
if self.__endEffector:
self.__endEffector.openEndEffector()
time.sleep(1)
self.__endEffector.urgentStop = value
@property
def killThread(self):
return self._killThread
@killThread.setter
def killThread(self, value):
""" Mark each link so their respective threads are killed (gyroscope + motion) """
for link in [link for link in self.__links if link.initOrder > 0]:
link.killThread = True
self._killThread = value
@property
def currentConfigVector(self):
config_list = []
for link in [link for link in self.__links if link.virtual is False]:
config_list.append(link.armAngleDegX)
# add the end effector
config_list.append(self.__endEffector.status)
return np.r_[config_list]
@property
def endEffectorStatus(self):
return self.__endEffector.status
# endregion
def __str__(self):
'''
Definition: returns a string containing the angles for easy printout (in debugging)
'''
curThetaString = ''
for link in [link for link in self.__links if link.virtual is False]:
curX = link.armAngleDegX
curThetaString += f'j{link.linkID}:{curX:.1f} - '
# add real angle (wrt vertical) - accessing real value - semi illegal... not elegant
for link in [link for link in self.__links if link.virtual is False]:
curX = link.gyroSensor.currentAngleXDeg
curThetaString += f'r{link.linkID}:{curX:.1f} - '
return curThetaString[:-2]
def resetArm(self):
'''
Description:resets the motors.
params:None
'''
log.debug(LOGGER_ARM_RESET, 'Resetting arm...')
for link in self.__links:
link.resetLink()
log.debug(LOGGER_ARM_RESET, 'Done.')
def moveToAngle(self,
armConfigVector,
dryrun=False):
'''
Description:meant to move links to position (in radians) at a certain fixed speed
param:targetAngleRadians:desc:target angle in radians
param:targetAngleRadians:type:[float]
param:dryrun:desc:if true, won't run, if false, will do.
param:dryrun:type:bool
'''
try:
self.__links.sort(key=lambda x: x.linkID)
physicalLinks = [link for link in self.__links if link.initOrder > 0]
self.__motionThreads = []
log.debug(LOGGER_ARM_MTA, 'Preparing the threads to move each arm link.')
for idx, link in enumerate(physicalLinks):
thread = threading.Thread(target=link.moveToAngle, args=([armConfigVector[idx]]), name="link.moveToAngle-{}".format(link.linkID))
thread.live = True
self.__motionThreads.append(thread)
if self.lock.locked():
log.info(LOGGER_ARM_MTA, 'Arm is locked. Waiting for unlock.')
else:
log.info(LOGGER_ARM_MTA, 'Arm is unlocked, proceeding.')
with self.lock:
log.info(LOGGER_ARM_MTA, 'Locked the arm : launching the thread for arm motion')
for thread in self.__motionThreads:
thread.start()
log.debug(LOGGER_ARM_MTA, 'Joining threads for arm motion')
for thread in self.__motionThreads:
thread.join()
log.debug(LOGGER_ARM_MTA, 'Threads joined...')
log.info(LOGGER_ARM_MTA, 'Unlocked Arm')
except:
log.error(LOGGER_ARM_MTA, 'Error : {}'.format(traceback.print_exc()))
raise
return
def openEndEffector(self, blocking=False, stop_action=STOP_ACTION_HOLD):
log.info(LOGGER_ARM_EE, 'Passing the call to the end effector to open')
if self.__endEffector:
self.__endEffector.openEndEffector(blocking=blocking, stop_action=stop_action)
def closeEndEffector(self, blocking=False, stop_action=STOP_ACTION_HOLD):
log.info(LOGGER_ARM_EE, 'Passing the call to the end effector to close')
if self.__endEffector:
self.__endEffector.closeEndEffector(blocking=blocking, stop_action=stop_action)
