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):
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):
#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 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.")
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
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
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 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
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
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()
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()
#!/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:
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()
# 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...
#----------------------------------------
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'
)
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
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)
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)
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