Ejemplo n.º 1
0
    def __init__(self, port="/dev/ttyUSB0", baud=1000000):
        ArbotiX.__init__(self, port, baud)
        print "--- Test Dynamixel Communication ---"
        print "This test use arbitix_ros package"

        while True:
            print ">>> ",
            keyboard_cmd = raw_input().split(" ")
            try:
                if keyboard_cmd[0] == "ping":
                    print "id:"
                elif keyboard_cmd[0] == "get":
                    if keyboard_cmd[1] == "position":
                        pos = self.getPosition(int(keyboard_cmd[2]))
                        if pos > 0:
                            print "position:%d" % pos
                        else:
                            print "error:" + str(pos)
                elif keyboard_cmd[0] == "set":
                    if keyboard_cmd[1] == "position":
                        index = int(keyboard_cmd[2])
                        pos = int(keyboard_cmd[3])
                        result = self.setPosition(index, pos)
                        print "callback:" + str(result)
            except Exception as exce:
                print "Error!", exce
Ejemplo n.º 2
0
    def __init__(self):
        #Initialize arbotix comunications
        print"\nArbotix initialization, wait 10 seconds..."
        ArbotiX.__init__(self)
        for x in xrange(1,21):
            time.sleep(0.5)
            print(str(x*0.5) + "/10s")



        #Setupr velocities and positions vectors and messages
        self.joints_poses = [0,0,0,0,0,0]
        self.joints_vels = [0,0,0,0,0]
        self.ee_closed = 0
        self.vels_to_pub = Float32MultiArray()
        self.poses_to_pub = Float32MultiArray()
        self.poses_layout = MultiArrayDimension('joints_poses', 6, 0)
        self.vels_layout = MultiArrayDimension('joints_vels', 5, 0)
        self.poses_to_pub.layout.dim = [self.poses_layout]
        self.poses_to_pub.layout.data_offset = 0
        self.vels_to_pub.layout.dim = [self.vels_layout]
        self.vels_to_pub.layout.data_offset = 0

        #ROS pubblisher for joint velocities and positions
        self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1)
        self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1)

        print"Windowx_3link node created, whaiting for messages in:"
        print"      windowx_2links/torque"
        print"Publishing joints' positions and velocities in:"
        print"      /windowx_3links/joints_poses"
        print"      /windowx_3links/joints_vels"
        #Start publisher
        self.publish()
    def __init__(self, port="/dev/ttyUSB0", baud=1000000):
        ArbotiX.__init__(self, port, baud)
        print "--- Test Dynamixel Communication ---"
        print "This test use arbitix_ros package"

        while True:
            print ">>> ",
            keyboard_cmd = raw_input().split(" ")
            try:
                if keyboard_cmd[0] == "ping":
                    print "id:"
                elif keyboard_cmd[0] == "get":
                    if keyboard_cmd[1] == "position":
                        pos = self.getPosition(int(keyboard_cmd[2]))
                        if pos > 0:
                            print "position:%d" % pos
                        else:
                            print "error:" + str(pos)
                elif keyboard_cmd[0] == "set":
                    if keyboard_cmd[1] == "position":
                        index = int(keyboard_cmd[2])
                        pos = int(keyboard_cmd[3])
                        result = self.setPosition(index, pos)
                        print "callback:" + str(result)
            except Exception as exce:
                print "Error!", exce
Ejemplo n.º 4
0
    def __init__(self, port="/dev/ttyUSB0", baud=1000000):
        #Initialize arbotix comunications
        print"\nArbotix initialization, wait 10 seconds..."
        ArbotiX.__init__(self, port, baud)
        for x in xrange(1,21):
            time.sleep(0.5)
            print(str(x*0.5) + "/10s")

        print"Done."
        #Set inital torque limits
        print"Limiting torques"
        mx28_init_torque_limit = int(MX_TORQUE_STEPS/3)
        mx64_init_torque_limit = int(MX_TORQUE_STEPS/5)
        ax_init_torque_limit = int(AX_TORQUE_STEPS/5)

        self.setTorqueLimit(int(1), mx28_init_torque_limit)
        self.setTorqueLimit(int(2), mx64_init_torque_limit)
        self.setTorqueLimit(int(3), (mx64_init_torque_limit + 100)) #The 3rd servo needs more torque to contrast initial inertias
        self.setTorqueLimit(int(4), mx28_init_torque_limit)
        self.setTorqueLimit(int(5), ax_init_torque_limit)
        self.setTorqueLimit(int(6), ax_init_torque_limit)
        #Go to 0 position for each servo
        print"Going to initialization position..."
        self.setPosition(int(1), int(MX_POS_CENTER))
        self.setPosition(int(4), int(MX_POS_CENTER))
        self.setPosition(int(3), int(MX_POS_CENTER))
        self.setPosition(int(2), int(MX_POS_CENTER))
        self.setPosition(int(5), int(AX_POS_CENTER))
        self.setPosition(int(6), int(AX_POS_CENTER))

        print"Arm ready, setting up ROS topics..."

        #Setupr velocities and positions vectors and messages
        self.joints_poses = [0,0,0,0,0,0]
        self.joints_vels = [0,0,0,0,0]
        self.ee_closed = 0
        self.vels_to_pub = Float32MultiArray()
        self.poses_to_pub = Float32MultiArray()
        self.poses_layout = MultiArrayDimension('joints_poses', 6, 0)
        self.vels_layout = MultiArrayDimension('joints_vels', 5, 0)
        self.poses_to_pub.layout.dim = [self.poses_layout]
        self.poses_to_pub.layout.data_offset = 0
        self.vels_to_pub.layout.dim = [self.vels_layout]
        self.vels_to_pub.layout.data_offset = 0

        #ROS pubblisher for joint velocities and positions
        self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1)
        self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1)

        #ROS listener for control torues
        self.torque_sub = rospy.Subscriber('windowx_3links/torques', Float32MultiArray, self._torque_callback, queue_size=1)

        print"Windowx_3link node created, whaiting for messages in:"
        print"      windowx_2links/torque"
        print"Publishing joints' positions and velocities in:"
        print"      /windowx_3links/joints_poses"
        print"      /windowx_3links/joints_vels"
        #Start publisher
        self.publish()
Ejemplo n.º 5
0
def tourn_off_arm():
    ar = ArbotiX()
    print "Disabling servos please wait..."
    #Set all servos torque limits to 0
    for j in xrange(1, 7):
        ar.setTorqueLimit(j, 0)

    print("Servos disabled. Windowx_3link node closed.")
Ejemplo n.º 6
0
    def __init__(self, name, host, password, graphplan, actionplan, knowledge):
        self.communicator = Communicator()

        print "will init rosnode with name: " + name
        rospy.init_node(name)

        # set the speed of each servo
        device = ArbotiX("/dev/ttyUSB0")
        for i in range(0, 5):
            device.setSpeed(i, 20)

        self.arm = ArmPub(name)
        self.arm.goToPosition(ArmPub.POSITION_RELAX)

        behaviours = self.communicator.getBehaviours()
        behaviours.append(SystemCore())

        # does not use the overwritten SpadeAgent
        # calls directly the constructor of Spade Agent
        Agent.__init__(self, name, host, password, graphplan, actionplan,
                       behaviours)

        with open(knowledge) as knowledge:
            self.setData("knowledge", json.load(knowledge)[self.localName])
        self.setData("goals", {"pose": {}})
        self.setData("naoAid",
                     spade.AID.aid("nao1@" + host, ["xmpp://nao1@" + host]))
        other = self.getData("knowledge")["other"] + "@" + host
        otherAid = spade.AID.aid(other, ["xmpp://" + other])
        self.setData("otherTurtleAid", otherAid)
        self.setData("otherState", Goals.otherStatus["nonReady"])
        print self.getData("knowledge")

        # stores parameters received by waitMessageBehaviour
        # in case it is a plan
        self.plan = ""
        # stores the parameters of an order
        self.parameters = list()
        # stores who gave the order
        self.messageSender = ""
        self.name = name
Ejemplo n.º 7
0
 def __init__(self, port="/dev/ttyUSB0"):
     # creating an ArbotiX device (the arm) on port (normally "/dev/ttyUSB0")
     self.device = ArbotiX(port)
     
     joint_defaults = getJointsFromURDF()
     self.servos = list() # list of the joints
     
     joints = rospy.get_param('/arbotix/joints', dict())
     for name in sorted(joints.keys()):
         # pull data to convert from Angle to ticks
         n = "/arbotix/joints/"+ name +"/"
         ticks = rospy.get_param(n+"ticks", 1024)
         neutral = rospy.get_param(n+"neutral", ticks/2)
         invert = rospy.get_param(n+"invert", False)
         ranges = rospy.get_param(n+"range", 300)
         rad_per_tick = radians(ranges)/ticks
         
         # pull min and max angles for each servo
         min_angle, max_angle = getJointLimits(name, joint_defaults)
         serv = Servo(name, min_angle, max_angle, ticks, neutral, rad_per_tick, invert)
         self.servos.append(serv)
    def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
        super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
        self._redist_rate = rospy.Rate(50)

        self._arbotix = ArbotiX('/dev/ttyUSB0')
        assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
        assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"

        self._joint_lock = Lock()
        self._angles, self._velocities = {}, {}
        rospy.Subscriber("/joint_states", JointState, self._joint_callback)
        time.sleep(1)        

        self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
        self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)

        p.connect(p.DIRECT)
        widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
        self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) 
        p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))       
        self._n_errors = 0
Ejemplo n.º 9
0
    def __init__(self):
        # pause = False

        # load configurations    
        port = rospy.get_param("~port", "/dev/ttyUSB0")
        baud = int(rospy.get_param("~baud", "115200"))

        # self.rate = rospy.get_param("~rate", 100.0)
        # self.fake = rospy.get_param("~sim", False)

        # self.use_sync_read = rospy.get_param("~sync_read",True)      # use sync read?
        # self.use_sync_write = rospy.get_param("~sync_write",True)    # use sync write?

        # setup publishers
        # self.diagnostics = DiagnosticsPublisher()
        # self.joint_state_publisher = JointStatePublisher()

        # start an arbotix driver
        # if not self.fake:
        ArbotiX.__init__(self, port, baud)        
        rospy.sleep(1.0)
        rospy.loginfo("Started ArbotiX connection on port " + port + ".")
Ejemplo n.º 10
0
def main():
    CONV_H = 0.37
    CONV_V = 0.3
    dyna_center = 512
    arby = ArbotiX("/dev/ttyUSB0", 115200)
    pan_rng, tilt_rng = panorama.get_camera_angles(dyna_center, CONV_V, CONV_H)

    rospy.init_node('brinkmanship_core')
    signal(SIGINT, handler)
    camera_pub = rospy.Publisher('image_capture_message', String, queue_size=1)
    sub = rospy.Subscriber('edge_alert', String, edge_alert_callback)

    speed = 0.75
    turn = 1

    # bot_stop(0,0)
    # time.sleep(2)
    # bot_forward(speed, turn)
    # time.sleep(2)
    # bot_stop(0,0)
    panorama.set_camera_pose(arby, dyna_center, dyna_center - 51)
    bot_forward(speed, turn)
    flag = 0
    while not rospy.is_shutdown():
        if not alert:

            pass
        else:
            bot_stop(speed, turn)
            b = datetime.datetime.now()
            if flag == 0:
                print("Alert has been published")
                print("Time required for stopping {} microseconds".format(
                    (b - a).microseconds))
                print("Moving Pan Tilt Motors")
                for pan in pan_rng:
                    for tilt in tilt_rng:
                        panorama.set_camera_pose(arby, pan, tilt)
                        time.sleep(3)
                        camera_pub.publish(str(pan) + '_' + str(tilt))
                flag = 1
Ejemplo n.º 11
0
def panorama_method():
    CONV_H = 0.37
    CONV_V = 0.3
    dyna_center = 512
    arby = ArbotiX("/dev/ttyUSB0", 115200)
    pan_rng, tilt_rng = panorama.get_camera_angles(dyna_center, CONV_V, CONV_H)

    rospy.init_node('brinkmanship_core')
    signal(SIGINT, handler)
    camera_pub = rospy.Publisher('image_capture_message', String, queue_size=1)

    speed = 0.75
    turn = 1

    panorama.set_camera_pose(arby, dyna_center, dyna_center - 51)
    print("Alert has been published")
    print("Moving Pan Tilt Motors")
    for pan in pan_rng:
        for tilt in tilt_rng:
            panorama.set_camera_pose(arby, pan, tilt)
            time.sleep(3)
            camera_pub.publish(str(pan) + '_' + str(tilt))
    flag = 1
Ejemplo n.º 12
0
    def __init__(self, port="/dev/ttyUSB0", baud=115200):
        # start
        ArbotiX.__init__(self, port, baud)
        print "ArbotiX Terminal --- Version 0.1"
        print "Copyright 2011 Vanadium Labs LLC"

        # loop
        while True:
            print ">> ",
            kmd = raw_input().split(" ")
            try:
                if kmd[0] == "help":  # display help data
                    if len(kmd) > 1:  # for a particular command
                        if kmd[1] == "ls":
                            print help[3]
                        elif kmd[1] == "mv":
                            print help[4]
                        elif kmd[1] == "baud":
                            print help[5]
                        elif kmd[1] == "get":
                            print help[6]
                        elif kmd[1] == "set":
                            print help[7]
                        else:
                            print "help: unrecognized command"
                    else:
                        for h in help:
                            print h

                elif kmd[0] == "ls":  # list servos
                    self._ser.timeout = 0.25
                    if len(kmd) > 2:
                        self.write(253, P_BAUD_RATE,
                                   [self.convertBaud(int(kmd[1]))])
                        self.query()
                    self.query()

                elif kmd[0] == "mv":  # rename a servo
                    if self.write(int(kmd[1]), P_ID, [
                            int(kmd[2]),
                    ]) == 0:
                        print self.OKBLUE + "OK" + self.ENDC

                elif kmd[0] == "baud":  # set bus baud rate
                    self.write(253, P_BAUD_RATE,
                               [self.convertBaud(int(kmd[1]))])
                    print self.OKBLUE + "OK" + self.ENDC

                elif kmd[0] == "set":
                    if kmd[1] == "baud":
                        self.write(int(kmd[2]), P_BAUD_RATE,
                                   [self.convertBaud(int(kmd[3]))])
                        print self.OKBLUE + "OK" + self.ENDC
                    elif kmd[1] == "pos" or kmd[1] == "position":
                        self.setPosition(int(kmd[2]), int(kmd[3]))
                        print self.OKBLUE + "OK" + self.ENDC

                elif kmd[0] == "get":
                    if kmd[1] == "temp":
                        value = self.getTemperature(int(kmd[2]))
                        if value >= 60 or value < 0:
                            print self.FAIL + str(value) + self.ENDC
                        elif value > 40:
                            print self.WARNING + str(value) + self.ENDC
                        else:
                            print self.OKGREEN + str(value) + self.ENDC
                    elif kmd[1] == "pos" or kmd[1] == "position":
                        value = self.getPosition(int(kmd[2]))
                        if value >= 0:
                            print self.OKGREEN + str(value) + self.ENDC
                        else:
                            print self.FAIL + str(value) + self.ENDC

            except Exception as e:
                print "error...", e
Ejemplo n.º 13
0
    def __init__(self):
        pause = False

        # load configurations    
        port = rospy.get_param("~port", "/dev/ttyUSB0")
        baud = int(rospy.get_param("~baud", "115200"))

        self.rate = rospy.get_param("~rate", 100.0)
        self.fake = rospy.get_param("~sim", False)

        self.use_sync_read = rospy.get_param("~sync_read",True)      # use sync read?
        self.use_sync_write = rospy.get_param("~sync_write",True)    # use sync write?

        # setup publishers
        self.diagnostics = DiagnosticsPublisher()
        self.joint_state_publisher = JointStatePublisher()

        # start an arbotix driver
        if not self.fake:
            ArbotiX.__init__(self, port, baud)        
            rospy.sleep(1.0)
            rospy.loginfo("Started ArbotiX connection on port " + port + ".")
        else:
            rospy.loginfo("ArbotiX being simulated.")

        # initialize dynamixel & hobby servos
        self.servos = Servos(self)

        # setup controllers
        self.controllers = list()
        controllers = rospy.get_param("~controllers", dict())
        for name, params in controllers.items():
            if params["type"] == "follow_controller":
                self.controllers.append(FollowController(self, name))
                if self.controllers[-1].onboard:
                    pause = True
            elif params["type"] == "diff_controller":
                self.controllers.append(DiffController(self, name))
                pause = True
#           elif params["type"] == "omni_controller":
#               self.controllers.append(OmniController(self, name))
#               pause = True
            else:
                rospy.logerr("Unrecognized controller: " + params["type"])

        # wait for arbotix to start up (especially after reset)
        if not self.fake:
            if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"):
                pause = True
            if pause:
                while self.getDigital(1) == -1:
                    rospy.loginfo("Waiting for response...")
                    rospy.sleep(0.25)
            rospy.loginfo("ArbotiX connected.")

        for controller in self.controllers:
            controller.startup()

        # services for io
        rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb)
        rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb)
        rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb)
        # initialize digital/analog IO streams
        self.io = dict()
        if not self.fake:
            for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items():
                temp = rospy.get_param("~"+v,dict())
                for name in temp.keys():
                    pin = rospy.get_param('~'+v+'/'+name+'/pin',1)
                    value = rospy.get_param('~'+v+'/'+name+'/value',0)
                    rate = rospy.get_param('~'+v+'/'+name+'/rate',10)
                    self.io[name] = t(name, pin, value, rate, self)
        
        r = rospy.Rate(self.rate)

        # main loop -- do all the read/write here
        while not rospy.is_shutdown():
    
            # update controllers
            for controller in self.controllers:
                controller.update()

            # update servo positions (via sync_write)
            self.servos.update(self.use_sync_write)

            # update io
            for io in self.io.values():
                io.update()

            # publish
            self.servos.interpolate(self.use_sync_read)
            self.joint_state_publisher.update(self.servos, self.controllers)
            self.diagnostics.update(self.servos, self.controllers)

            r.sleep()

        # do shutdown
        for controller in self.controllers:
            controller.shutdown()
Ejemplo n.º 14
0
        ticks = serv.angleToTicks(0.7854)
        print ticks
        device.setPosition(idserv, ticks)

    def shutdown(self):
        # stop turtlebot
        rospy.loginfo("Stop TurtleBot")
        # a default Twist has linear.x of 0 and angular.z of 0.  So it'll stop TurtleBot
        self.cmd_vel.publish(Twist())
        # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
        rospy.sleep(1)


if __name__ == "__main__":
    rospy.init_node('arm')
    d = ArbotiX(port="/dev/ttyUSB0")
    arm = Arm()
    arm.newMoveServo(d, 2)
    '''
    # premiere methode... pas mal mais n'arrive pas a fixer la vitesse
    arm = Arm()
    positions = [0,0,0,0,0,0]
    forward = 5
    turn = 5
    pos = 0.0
    for i in range(0,10):
        arm.moveServo(pos)
        time.sleep(0.5)
        pos = pos + 0.1

    def __init__(self):
        pause = False

        # load configurations    
        port = rospy.get_param("~port", "/dev/ttyUSB0")
        baud = int(rospy.get_param("~baud", "115200"))

        self.rate = rospy.get_param("~rate", 100.0)
        self.fake = rospy.get_param("~sim", False)

        self.use_sync_read = rospy.get_param("~sync_read",True)      # use sync read?
        self.use_sync_write = rospy.get_param("~sync_write",True)    # use sync write?

        # setup publishers
        self.diagnostics = DiagnosticsPublisher()
        self.joint_state_publisher = JointStatePublisher()

        # start an arbotix driver
        if not self.fake:
            ArbotiX.__init__(self, port, baud)        
            rospy.sleep(1.0)
            rospy.loginfo("Started ArbotiX connection on port " + port + ".")
        else:
            rospy.loginfo("ArbotiX being simulated.")

        # setup joints
        self.joints = dict()
        for name in rospy.get_param("~joints", dict()).keys():
            joint_type = rospy.get_param("~joints/"+name+"/type", "dynamixel")
            if joint_type == "dynamixel":
                self.joints[name] = DynamixelServo(self, name)
            elif joint_type == "hobby_servo":
                self.joints[name] = HobbyServo(self, name)
            elif joint_type == "calibrated_linear":
                self.joints[name] = LinearJoint(self, name)

    # TODO: <BEGIN> REMOVE THIS BEFORE 1.0
        if len(rospy.get_param("~dynamixels", dict()).keys()) > 0:
            rospy.logwarn("Warning: use of dynamixels as a dictionary is deprecated and will be removed in 0.8.0")
            for name in rospy.get_param("~dynamixels", dict()).keys():
                self.joints[name] = DynamixelServo(self, name, "~dynamixels")
        if len(rospy.get_param("~servos", dict()).keys()) > 0:
            rospy.logwarn("Warning: use of servos as a dictionary is deprecated and will be removed in 0.8.0")
            for name in rospy.get_param("~servos", dict()).keys():
                self.joints[name] = HobbyServo(self, name, "~servos")
    # TODO: <END> REMOVE THIS BEFORE 1.0

        # setup controller
        self.controllers = [ServoController(self, "servos"), ]
        controllers = rospy.get_param("~controllers", dict())
        for name, params in controllers.items():
            try:
                controller = controller_types[params["type"]](self, name)
                self.controllers.append( controller )
                pause = pause or controller.pause
            except Exception as e:
                if type(e) == KeyError:
                    rospy.logerr("Unrecognized controller: " + params["type"])
                else:  
                    rospy.logerr(str(type(e)) + str(e))

        # wait for arbotix to start up (especially after reset)
        if not self.fake:
            if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"):
                pause = True
            if pause:
                while self.getDigital(1) == -1 and not rospy.is_shutdown():
                    rospy.loginfo("Waiting for response...")
                    rospy.sleep(0.25)
            rospy.loginfo("ArbotiX connected.")

        for controller in self.controllers:
            controller.startup()

        # services for io
        rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb)
        rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb)
        rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb)
        # initialize digital/analog IO streams
        self.io = dict()
        if not self.fake:
            for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items():
                temp = rospy.get_param("~"+v,dict())
                for name in temp.keys():
                    pin = rospy.get_param('~'+v+'/'+name+'/pin',1)
                    value = rospy.get_param('~'+v+'/'+name+'/value',0)
                    rate = rospy.get_param('~'+v+'/'+name+'/rate',10)
                    self.io[name] = t(name, pin, value, rate, self)
        
        r = rospy.Rate(self.rate)

        # main loop -- do all the read/write here
        while not rospy.is_shutdown():
    
            # update controllers
            for controller in self.controllers:
                controller.update()

            # update io
            for io in self.io.values():
                io.update()

            # publish feedback
            self.joint_state_publisher.update(self.joints.values(), self.controllers)
            self.diagnostics.update(self.joints.values(), self.controllers)

            r.sleep()

        # do shutdown
        for controller in self.controllers:
            controller.shutdown()
Ejemplo n.º 16
0
#!/usr/bin/env python
# license removed for brevity
import rospy
from arbotix_python.arbotix import ArbotiX
from arbotix_python.ax12 import *
from sensor_msgs.msg import Joy

turret = ArbotiX()


def scan():
    # Set speed
    print turret.setSpeed(1, 50)
    print turret.setSpeed(2, 50)

    print turret.getSpeed(1)
    # Put in initial position
    turret.setPosition(1, 510)
    turret.setPosition(2, 510)
    while turret.isMoving(1) or turret.isMoving(2):
        rospy.sleep(0.5)

    raw_input("Please enter something")

    # Do a scan cycle
    currentPosition = 0
    for value in range(400, 600, 50):
        turret.setPosition(2, value)
        while turret.isMoving(2):
            rospy.sleep(0.5)
        if currentPosition == 0:
Ejemplo n.º 17
0
    def __init__(self):
        pause = False

        # load configurations    
        port = rospy.get_param("~port", "/dev/ttyUSB0")
        baud = int(rospy.get_param("~baud", "115200"))

        self.rate = rospy.get_param("~rate", 100.0)
        self.fake = rospy.get_param("~sim", False)

        self.use_sync_read = rospy.get_param("~sync_read",True)      # use sync read?
        self.use_sync_write = rospy.get_param("~sync_write",True)    # use sync write?

        # setup publishers
        self.diagnostics = DiagnosticsPublisher()
        self.joint_state_publisher = JointStatePublisher()

        # start an arbotix driver
        if not self.fake:
            ArbotiX.__init__(self, port, baud)        
            rospy.sleep(1.0)
            rospy.loginfo("Started ArbotiX connection on port " + port + ".")
        else:
            rospy.loginfo("ArbotiX being simulated.")

        # setup joints
        self.joints = dict()
        for name in rospy.get_param("~joints", dict()).keys():
            joint_type = rospy.get_param("~joints/"+name+"/type", "dynamixel")
            if joint_type == "dynamixel":
                self.joints[name] = DynamixelServo(self, name)
            elif joint_type == "hobby_servo":
                self.joints[name] = HobbyServo(self, name)
            elif joint_type == "calibrated_linear":
                self.joints[name] = LinearJoint(self, name)

    # TODO: <BEGIN> REMOVE THIS BEFORE 1.0
        if len(rospy.get_param("~dynamixels", dict()).keys()) > 0:
            rospy.logwarn("Warning: use of dynamixels as a dictionary is deprecated and will be removed in 0.8.0")
            for name in rospy.get_param("~dynamixels", dict()).keys():
                self.joints[name] = DynamixelServo(self, name, "~dynamixels")
        if len(rospy.get_param("~servos", dict()).keys()) > 0:
            rospy.logwarn("Warning: use of servos as a dictionary is deprecated and will be removed in 0.8.0")
            for name in rospy.get_param("~servos", dict()).keys():
                self.joints[name] = HobbyServo(self, name, "~servos")
    # TODO: <END> REMOVE THIS BEFORE 1.0

        # setup controller
        self.controllers = [ServoController(self, "servos"), ]
        controllers = rospy.get_param("~controllers", dict())
        for name, params in controllers.items():
            try:
                controller = controller_types[params["type"]](self, name)
                self.controllers.append( controller )
                pause = pause or controller.pause
            except Exception as e:
                if type(e) == KeyError:
                    rospy.logerr("Unrecognized controller: " + params["type"])
                else:  
                    rospy.logerr(str(type(e)) + str(e))

        # wait for arbotix to start up (especially after reset)
        if not self.fake:
            if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"):
                pause = True
            if pause:
                while self.getDigital(1) == -1 and not rospy.is_shutdown():
                    rospy.loginfo("Waiting for response...")
                    rospy.sleep(0.25)
            rospy.loginfo("ArbotiX connected.")

        for controller in self.controllers:
            controller.startup()

        # services for io
        rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb)
        rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb)
        rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb)
        # initialize digital/analog IO streams
        self.io = dict()
        if not self.fake:
            for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items():
                temp = rospy.get_param("~"+v,dict())
                for name in temp.keys():
                    pin = rospy.get_param('~'+v+'/'+name+'/pin',1)
                    value = rospy.get_param('~'+v+'/'+name+'/value',0)
                    rate = rospy.get_param('~'+v+'/'+name+'/rate',10)
                    self.io[name] = t(name, pin, value, rate, self)
        
        r = rospy.Rate(self.rate)

        # main loop -- do all the read/write here
        while not rospy.is_shutdown():
    
            # update controllers
            for controller in self.controllers:
                controller.update()

            # update io
            for io in self.io.values():
                io.update()

            # publish feedback
            self.joint_state_publisher.update(self.joints.values(), self.controllers)
            self.diagnostics.update(self.joints.values(), self.controllers)

            r.sleep()

        # do shutdown
        for controller in self.controllers:
            controller.shutdown()
Ejemplo n.º 18
0
# Comment or uncomment prints for reading or writing servo parameters.

from arbotix_python.arbotix import ArbotiX
from time import sleep

port = "/dev/ttyUSB0"

arbotix = ArbotiX(port)

### To read one parameter of (one/a lot) servo/s.
### for info visit http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/#control-table-of-eeprom-area

# List of servos IDs
servos = [1, 2, 3, 4, 5]
#servos = [5]

# Value of the register (EEPROM or RAM area) to read
# parameter to set and its size
data_name = 36
size = 2
while True:
    print arbotix.syncRead(servos, data_name, size)
    sleep(0.1)
#print arbotix.syncRead(servos,data_name,size)

# Return example (with Max Torque):
# >>> [100, 1, 255, 3, 100, 1, 100, 1, 100, 1]
# It means, Servo 1 has a value of 256*1 + 100
# Servo 2 has a value of 256*3 + 255...

#----------------------------------------
Ejemplo n.º 19
0
from arbotix_python.arbotix import ArbotiX

SERVO_IDS = [1, 2, 3, 4, 5, 6]

arbotix = ArbotiX('/dev/ttyUSB0')
err = arbotix.syncWrite(14, [[servo_id, 255, 1] for servo_id in SERVO_IDS])

if err != -1:
    print('Successfully reconfigured servos')
else:
    print(
        'Error writing to servos -- make sure start.sh is not running in the background'
    )
Ejemplo n.º 20
0
class WidowXController(RobotController):
    def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
        super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
        self._redist_rate = rospy.Rate(50)

        self._arbotix = ArbotiX('/dev/ttyUSB0')
        assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
        assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"

        self._joint_lock = Lock()
        self._angles, self._velocities = {}, {}
        rospy.Subscriber("/joint_states", JointState, self._joint_callback)
        time.sleep(1)        

        self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
        self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)

        p.connect(p.DIRECT)
        widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
        self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) 
        p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))       
        self._n_errors = 0

    def _init_gripper(self, gripper_attached):
        assert gripper_attached == 'default', 'widow_x only supports its default gripper at the moment'

    def _joint_callback(self, msg):
        with self._joint_lock:
            for name, position, velocity in zip(msg.name, msg.position, msg.velocity):
                self._angles[name] = position
                self._velocities[name] = velocity
    
    def _move_to_target_joints(self, joint_values):
        for value, pub in zip(joint_values, self._joint_pubs):
            pub.publish(Float64(value))

    def move_to_neutral(self, duration=2):
        self._n_errors = 0
        self._lerp_joints(np.array(NEUTRAL_VALUES), duration)
        self._gripper_pub.publish(Float64(GRIPPER_DROP[0]))
        time.sleep(GRIPPER_WAIT)
    
    def _reset_pybullet(self): 
        for i, angle in enumerate(self.get_joint_angles()):
            p.resetJointState(self._armID, i, angle)

    def _lerp_joints(self, target_joint_pos, duration):
        start_t, start_joints = rospy.get_time(), self.get_joint_angles()
        self._control_rate.sleep()
        cur_joints = self.get_joint_angles()
        while rospy.get_time() - start_t < 1.2 * duration and not np.isclose(target_joint_pos[:5], cur_joints[:5], atol=CONTROL_TOL).all():
            t = min(1.0, (rospy.get_time() - start_t) / duration)
            target_joints = (1 - t) * start_joints[:5] + t * target_joint_pos[:5]
            self._move_to_target_joints(target_joints)

            self._control_rate.sleep()
            cur_joints = self.get_joint_angles()
        logging.getLogger('robot_logger').debug('Lerped for {} seconds'.format(rospy.get_time() - start_t))
        self._reset_pybullet()

        delta = np.linalg.norm(target_joints[:5] - cur_joints[:5])
        if delta > MAX_FINAL_ERR:
            assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
            self._n_errors += 1
        if self._n_errors > MAX_ERRORS:
            logging.getLogger('robot_logger').error('More than {} errors! WidowX probably crashed.'.format(MAX_ERRORS))
            raise Environment_Exception
        
        logging.getLogger('robot_logger').debug('Delta at end of lerp is {}'.format(delta))
        
    
    def move_to_eep(self, target_pose, duration=1.5):
        """
        :param target_pose: Cartesian pose (x,y,z, quat).
        :param duration: Total time trajectory will take before ending
        """
        target_joint_pos = self._calculate_ik(target_pose[:3], target_pose[3:])[0]
        target_joint_pos = np.clip(np.array(target_joint_pos)[:6], JOINT_MIN, JOINT_MAX)
        self._lerp_joints(target_joint_pos, duration)

    def move_to_ja(self, waypoints, duration=1.5):
        """
        :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
                                                      Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
        :param duration: Total time trajectory will take before ending
        """
        for waypoint in waypoints:
            # Move to each waypoint in sequence. See move_to_target_joint_position 
            # in controller.py 
            self._move_to_target_joints(waypoint)

    def redistribute_objects(self):
        """
        Play pre-recorded trajectory that sweeps objects into center of bin
        """
        logging.getLogger('robot_logger').info('redistribute...')
        file = '/'.join(str.split(robot_envs.__file__, "/")[
                        :-1]) + '/recorded_trajectories/pushback_traj_{}.pkl'.format(self._robot_name)
        joint_pos = pkl.load(open(file, "rb"))

        self._redist_rate.sleep()
        for joint_t in joint_pos:
            print(joint_t)
            self._move_to_target_joints(joint_t)
            self._redist_rate.sleep()

    def get_joint_angles(self):
        #returns current joint angles
        with self._joint_lock:
            joints_ret = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'gripper_revolute_joint']
            try:
                return np.array([self._angles[k] for k in joints_ret])
            except KeyError:
                return None

    def get_joint_angles_velocity(self):
        #returns current joint angles
        #No velocities for widowx? :(
        with self._joint_lock:
            joints_ret = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'gripper_revolute_joint']
            try:
                return np.array([self._velocities[k] for k in joints_ret])
            except KeyError:
                return None

    def get_cartesian_pose(self):
        #Returns cartesian end-effector pose
        self._reset_pybullet()
        position, quat = p.getLinkState(self._armID, 5, computeForwardKinematics=1)[4:6]
        return np.array(list(position) + list(quat))
    
    def quat_2_euler(self, quat):
        roll, pitch, yaw = Quaternion(quat).yaw_pitch_roll
        return np.pi - yaw, pitch, roll

    def euler_2_quat(self, yaw=0.0, pitch=np.pi, roll=0.0):
        roll_matrix = np.array([[np.cos(roll), -np.sin(roll), 0.0],[np.sin(roll), np.cos(roll), 0.0], [0, 0, 1.0]])
        pitch_matrix = np.array([[np.cos(pitch), 0., np.sin(pitch)], [0.0, 1.0, 0.0], [-np.sin(pitch), 0, np.cos(pitch)]])
        yaw_matrix = np.array([[1.0, 0, 0], [0, np.cos(yaw), -np.sin(yaw)], [0, np.sin(yaw), np.cos(yaw)]])
        rot_mat = roll_matrix.dot(pitch_matrix.dot(yaw_matrix))
        return Quaternion(matrix=rot_mat).elements

    def get_gripper_state(self, integrate_force=False):                         
        # returns gripper joint angle, force reading (none if no force)
        with self._joint_lock:
            joint_angle = self._angles['gripper_prismatic_joint_1']
        return joint_angle, None

    def open_gripper(self, wait = False):                                       # should likely wrap separate gripper control class for max re-usability
        self._gripper_pub.publish(Float64(self.GRIPPER_OPEN))
        time.sleep(GRIPPER_WAIT)

    def close_gripper(self, wait = False):                                      # should likely wrap separate gripper control class for max re-usability
        self._gripper_pub.publish(Float64(self.GRIPPER_CLOSE))
        time.sleep(GRIPPER_WAIT)

    @property
    def GRIPPER_CLOSE(self):
        return GRIPPER_CLOSED[0]
    
    @property
    def GRIPPER_OPEN(self):
        return GRIPPER_OPEN[1]

    def _calculate_ik(self, targetPos, targetQuat, threshold=1e-5, maxIter=1000, nJoints=6):
        closeEnough = False
        iter_count = 0
        dist2 = None
        
        best_ret, best_dist = None, float('inf')

        while (not closeEnough and iter_count < maxIter):
            jointPoses = list(p.calculateInverseKinematics(self._armID, 5, targetPos, targetQuat, JOINT_MIN, JOINT_MAX))
            for i in range(nJoints):
                jointPoses[i] = max(min(jointPoses[i], JOINT_MAX[i]), JOINT_MIN[i])
                p.resetJointState(self._armID, i, jointPoses[i])
            
            ls = p.getLinkState(self._armID, 5, computeForwardKinematics=1)
            newPos, newQuat = ls[4], ls[5]
            dist2 = sum([(targetPos[i] - newPos[i]) ** 2 for i in range(3)])
            closeEnough = dist2 < threshold
            iter_count += 1
            
            if dist2 < best_dist:
                best_ret, best_dist = (jointPoses, newPos, newQuat), dist2
        
        return best_ret 
Ejemplo n.º 21
0
	if ('Utah_Pit' in file_locations['robot_simulation_env']):
		TIME_OUT = 226*3+ 162*12+ 355*3+ 500*5-300
		#TIME_OUT = 2200*4.1 #one shot 2200*1.3, #3 shots = 2200*3
	if ('Utah_BIG' in file_locations['robot_simulation_env']):
		TIME_OUT = 2200*1 #normal = 1.3 big = 1 
	if ('Lacus_Mortis_Pit' in file_locations['robot_simulation_env']):
		TIME_OUT = 2200*10
	if ('Pit_Edge_Test' in file_locations['robot_simulation_env']):
		TIME_OUT = 4*160+160 #normal = 1.3 big = 1  
else:
	# sys.path.insert(1,file_locations['robot_simulation_env'][0:file_locations['robot_simulation_env'].rfind("/",0,-1)]+"/Brinkmanship")
	# from cloud_process import CloudSubscriber
	# from brinkmanship import Brinkmanship
	from arbotix_python.arbotix import ArbotiX
	TIME_OUT = 80+3*160+240 + rospy.get_rostime().secs
	arb = ArbotiX("/dev/ttyUSB0",115200)
	arb = ArbotiX("/dev/ttyUSB1",115200)


img_capture_pub = rospy.Publisher('/image_number', Int32, queue_size = 10)

map_resolution = rospy.get_param("/resolution")
halfway_point = len(smach_helper.read_csv_around_pit(file_locations['file_around_pit'],map_resolution))/2

waypoint_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size = 1)
pit_edge_dist_pub = rospy.Publisher('/robot_at_edge_position', Odometry, queue_size = 1)

move_base_client = actionlib.SimpleActionClient('move_base',MoveBaseAction)

sub_where_to_see = rospy.Subscriber('where_to_see',Float32,smach_helper.update_sun)
Ejemplo n.º 22
0
class Arm:
    
    DEFAULT_SPEED = 20
    # predifined positions. More an be added the same way
    # a list of angles for each servo (first one is the gripper then the motors in order)
    # from these positions we can define variants:
    # with gripper closed. First parameter = 2.6
    # on the left 90. Second parameter = PI/2
    # on the right 90. Second parameter = -PI/2
    POSITION_1 = [0,0,0.38,0.28, 0.88, -1.7]
    POSITION_2 = [0,0,1.07,-0.59, 0.88, -1.7]
    POSITION_RELAX = [0,0,-1.21,1.5,0.64, 0]
    POSITION_BRING = [2.6,0,-0.57,0.12,1.5,-1.7]
    
    def __init__(self, port="/dev/ttyUSB0"):
        # creating an ArbotiX device (the arm) on port (normally "/dev/ttyUSB0")
        self.device = ArbotiX(port)
        
        joint_defaults = getJointsFromURDF()
        self.servos = list() # list of the joints
        
        joints = rospy.get_param('/arbotix/joints', dict())
        for name in sorted(joints.keys()):
            # pull data to convert from Angle to ticks
            n = "/arbotix/joints/"+ name +"/"
            ticks = rospy.get_param(n+"ticks", 1024)
            neutral = rospy.get_param(n+"neutral", ticks/2)
            invert = rospy.get_param(n+"invert", False)
            ranges = rospy.get_param(n+"range", 300)
            rad_per_tick = radians(ranges)/ticks
            
            # pull min and max angles for each servo
            min_angle, max_angle = getJointLimits(name, joint_defaults)
            serv = Servo(name, min_angle, max_angle, ticks, neutral, rad_per_tick, invert)
            self.servos.append(serv)
        
    # moves a single servo at a given speed to a given angle (in radians)
    # by default the speed is set at 20
    def moveServo(self, idserv, angle , speed=DEFAULT_SPEED):
        serv = self.servos[idserv]
        self.device.setSpeed(idserv, speed)
        ticks = serv.angleToTicks(angle)
        self.device.setPosition(idserv, ticks)
    
    
    def goToPosition(self, position):
        i = 0
        for pos in position:
            print "moving servo: ", i
            self.moveServo(i,pos)
            i = i+1
            
    def closeGripper(self):
        min_angle = self.servos[0].min_angle
        print " min angle for gripper = " , min_angle
        self.moveServo(0,min_angle)
    
    def openGripper(self):
        max_angle = self.servos[0].max_angle
        print " max angle for gripper = " , max_angle
        self.moveServo(0,max_angle)
        
    def take(self):
        
        self.openGripper()
        time.sleep(2)
        self.goToPosition(self.POSITION_1)
        # waits 2 seconds before closing the gripper
        time.sleep(2)
        self.closeGripper()
        # waits 2 seconds before taking the object off the support
        time.sleep(2)
        self.goToPosition(Arm.POSITION_BRING)
        
    def put(self):
        self.goToPosition(self.POSITION_1)
        # waits 2 seconds before closing the gripper
        time.sleep(2)
        self.openGripper()
        # waits 2 seconds before taking the object off the support
        time.sleep(2)
        self.goToPosition(Arm.POSITION_BRING)
        
Ejemplo n.º 23
0
    def __init__(self, port = "/dev/ttyUSB0", baud = 115200):
        # start
        ArbotiX.__init__(self, port, baud)  
        print "ArbotiX Terminal --- Version 0.1"
        print "Copyright 2011 Vanadium Labs LLC"

        # loop
        while True:
            print ">> ",
            kmd = raw_input().split(" ")
            try:
                if kmd[0] == "help":        # display help data
                    if len(kmd) > 1:        # for a particular command
                        if kmd[1] == "ls":   
                            print help[3]
                        elif kmd[1] == "mv":
                            print help[4]
                        elif kmd[1] == "baud":
                            print help[5]
                        elif kmd[1] == "get":
                            print help[6]
                        elif kmd[1] == "set":   
                            print help[7]
                        else:
                            print "help: unrecognized command"
                    else:
                        for h in help:
                            print h

                elif kmd[0] == "ls":       # list servos
                    self._ser.timeout = 0.25
                    if len(kmd) > 2:
                        self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))])
                        self.query()
                    self.query()

                elif kmd[0] == "mv":         # rename a servo
                    if self.write( int(kmd[1]), P_ID, [int(kmd[2]),] ) == 0:
                        print self.OKBLUE+"OK"+self.ENDC

                elif kmd[0] == "baud":      # set bus baud rate
                    self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))])
                    print self.OKBLUE+"OK"+self.ENDC

                elif kmd[0] == "set":
                    if kmd[1] == "baud":
                        self.write( int(kmd[2]), P_BAUD_RATE, [self.convertBaud(int(kmd[3]))] )
                        print self.OKBLUE+"OK"+self.ENDC
                    elif kmd[1] == "pos" or kmd[1] == "position":
                        self.setPosition( int(kmd[2]), int(kmd[3]) )
                        print self.OKBLUE+"OK"+self.ENDC

                elif kmd[0] == "get":
                    if kmd[1] == "temp":
                        value = self.getTemperature(int(kmd[2]))
                        if value >= 60 or value < 0:
                            print self.FAIL+str(value)+self.ENDC
                        elif value > 40:
                            print self.WARNING+str(value)+self.ENDC
                        else:
                            print self.OKGREEN+str(value)+self.ENDC
                    elif kmd[1] == "pos" or kmd[1] == "position":
                        value = self.getPosition(int(kmd[2]))
                        if value >= 0:
                            print self.OKGREEN+str(value)+self.ENDC
                        else:
                            print self.FAIL+str(value)+self.ENDC
        
            except Exception as e:
                print "error...", e