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
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
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):
#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()
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 + ".")
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
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()
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
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()
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()
