예제 #1
0
 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.')
예제 #2
0
파일: micohand.py 프로젝트: romesc-CMU/prpy
    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)
예제 #3
0
    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))
예제 #4
0
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)