class Detector():
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber("/head_mount_kinect/rgb/image_rect_color", Image, self.cb, queue_size=1)
self.pub = rospy.Publisher("blob_loc", String, queue_size=1)
self.uc = Uber()
rospy.loginfo("Detector initialized.")
def cb(self, data):
#Detect green box!
params = cv2.SimpleBlobDetector_Params()
params.filterByArea = True
params.filterByCircularity = False
params.filterByConvexity = False
params.minArea = 150
params.maxArea = 10e20
lower_blue = np.array([40,100,100])
upper_blue = np.array([80,255,255])
image_cv = self.bridge.imgmsg_to_cv2(data)
blur = cv2.GaussianBlur(image_cv,(41,41),0)
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
im = cv2.inRange(hsv, lower_blue, upper_blue)
im = cv2.bitwise_not(im)
detect = cv2.SimpleBlobDetector(params)
keypoints = detect.detect(im)
im_with_keypoints = cv2.drawKeypoints(im, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
#Head tracks green box, keeping it at coordinates [300, 300]
self.uc.command_head([clamp_head((300 - keypoints[0].pt[0]) / 100), clamp_head(-(300 - keypoints[0].pt[1]) / 100)], time=1, blocking=False)
self.pub.publish(str((300 - keypoints[0].pt[0]) / 100) + " " + str((300 - keypoints[0].pt[1]) / 100))
def clamp_head(n):
return max(-1.5, min(n, 1.5)) #So the head doesn't swing too far
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber("/head_mount_kinect/rgb/image_rect_color", Image, self.cb, queue_size=1)
self.pub = rospy.Publisher("blob_loc", String, queue_size=1)
self.uc = Uber()
rospy.loginfo("Detector initialized.")
class LeftGrip:
def __init__(self):
self.uc = Uber()
self.uc.open_gripper("l")
self.position = INITIAL_POS
self.gripping = False
self.left_gripper_sac = LeftGripper()
self.sub = rospy.Subscriber("pressure/l_gripper_motor", PS, self.callback, queue_size=1)
rospy.loginfo("Initialized grip detector.")
def callback(self, data):
left = np.array(data.l_finger_tip)
right = np.array(data.r_finger_tip)
l_mean = np.mean(left)
r_mean = np.mean(right)
# Calibration of default pressures
# rospy.loginfo(l_mean)
# rospy.loginfo(r_mean)
# check if there is pressure on a finger tip
if l_mean > L_STANDARD or r_mean > R_STANDARD:
self.gripping = True
# close grippers slowly if enough pressure
if self.gripping:
self.position = self.position - POS_CHANGE
if self.position < 0:
response = raw_input("Ready to release?")
self.position = INITIAL_POS
self.uc.command_gripper("l", self.position, -1, blocking=True, timeout=2)
self.gripping = False
self.left_gripper_sac.grip(self.position, 15)
rospy.sleep(0.01)
def __init__(self):
self.uc = Uber()
self.uc.open_gripper("l")
self.position = INITIAL_POS
self.gripping = False
self.left_gripper_sac = LeftGripper()
self.sub = rospy.Subscriber("pressure/l_gripper_motor",
PS,
self.callback,
queue_size=1)
rospy.loginfo("Initialized grip detector.")
def __init__(self):
self.bridge = CvBridge()
self.sub_image = rospy.Subscriber("/head_mount_kinect/rgb/image_rect_color",
Image, self.cb, queue_size=1)
self.head_sac = Head()
self.uc = Uber()
rospy.loginfo("Detector initialized.")
def __init__(self):
self.uc = Uber()
self.uc.open_gripper("l")
self.position = INITIAL_POS
self.gripping = False
self.left_gripper_sac = LeftGripper()
self.sub = rospy.Subscriber("pressure/l_gripper_motor", PS, self.callback, queue_size=1)
rospy.loginfo("Initialized grip detector.")
def get_dance_configs():
uc = Uber()
done = False
while not done:
raw_input('When robot is in desired config press ENTER')
r_config = uc.get_joint_positions('r')
print 'The current right arm config is: ', r_config
l_config = uc.get_joint_positions('l')
print 'The current left arm config is: ', l_config
head_config = uc.get_head_pose()
print 'The current head config is: ', head_config
answer = raw_input('Enter y if done, else hit ENTER')
if answer == 'y':
done = True
def get_dance_configs():
uc = Uber()
done = False
while not done:
raw_input('When robot is in desired config press ENTER')
r_config = uc.get_joint_positions('r')
print 'The current right arm config is: ', r_config
l_config = uc.get_joint_positions('l')
print 'The current left arm config is: ', l_config
head_config = uc.get_head_pose()
print 'The current head config is: ', head_config
answer = raw_input('Enter y if done, else hit ENTER')
if answer == 'y':
done = True
class LeftGrip:
def __init__(self):
self.uc = Uber()
self.uc.open_gripper("l")
self.position = INITIAL_POS
self.gripping = False
self.left_gripper_sac = LeftGripper()
self.sub = rospy.Subscriber("pressure/l_gripper_motor",
PS,
self.callback,
queue_size=1)
rospy.loginfo("Initialized grip detector.")
def callback(self, data):
left = np.array(data.l_finger_tip)
right = np.array(data.r_finger_tip)
l_mean = np.mean(left)
r_mean = np.mean(right)
# Calibration of default pressures
# rospy.loginfo(l_mean)
# rospy.loginfo(r_mean)
# check if there is pressure on a finger tip
if l_mean > L_STANDARD or r_mean > R_STANDARD:
self.gripping = True
# close grippers slowly if enough pressure
if self.gripping:
self.position = self.position - POS_CHANGE
if self.position < 0:
response = raw_input("Ready to release?")
self.position = INITIAL_POS
self.uc.command_gripper("l",
self.position,
-1,
blocking=True,
timeout=2)
self.gripping = False
self.left_gripper_sac.grip(self.position, 15)
rospy.sleep(0.01)
uc.down_torso(False)
uc.open_gripper('l', False)
uc.open_gripper('r', False)
nod()
uc.close_gripper('l', False)
uc.close_gripper('r', False)
nod()
uc.open_gripper('l', False)
uc.open_gripper('r', False)
nod()
uc.close_gripper('l', False)
uc.close_gripper('r', False)
nod()
uc.open_gripper('l', False)
uc.open_gripper('r')
uc.close_gripper('l', False)
uc.close_gripper('r', False)
nod()
uc.open_gripper('l', False)
uc.open_gripper('r')
uc.close_gripper('l', False)
uc.close_gripper('r', False)
nod()
rospy.init_node("ubertest")
rospy.loginfo("Dance")
uc = Uber()
reset()
dance()
elif string_msg.data == "green":
speak("yeah baby")
for _ in range(2):
self.left_arm.move([1.5, -1, 3, 1, 0, 0, 0], time=1, blocking=False)
self.right_arm.move([-1.5, -1,-3, 1, 0, 0, 0], time=1, blocking=False)
time.sleep(1)
self.left_arm.move([1.5, 0.3, 3, 0, 0, 0, 0], time=1, blocking=False)
self.right_arm.move([-1.5, 0.3,-3, 0, 0, 0, 0], time=1, blocking=False)
time.sleep(1)
else:
speak("show me something")
self.cnt+=1
if self.cnt == 10:
self.subscriber.unregister()
if __name__ == "__main__":
rospy.init_node("corlor_detector")
rospy.loginfo('Node initialized. Yeah, Baby!')
uc = Uber()
uc.command_head([0, 1], time=1, blocking=False)
time.sleep(1)
uc.close_gripper('l')
uc.close_gripper('r')
# whether blocking is true really does not matter here
uc.command_joint_pose('l', [1.5, 0.3, 3, 0, 0, 0, 0], time=1, blocking=True)
uc.command_joint_pose('r', [-1.5, 0.3,-3, 0, 0, 0, 0], time=1, blocking=True)
detector = Detector()
controller = MotionController()
rospy.spin()
#!/usr/bin/env python
#Author: Ivan Jutamulia
from lis_pr2_pkg.uber_controller import Uber
import rospy
import time
rospy.init_node("Dance_test")
rospy.loginfo("Ivan's dance")
uc = Uber()
# finding position parameters
training = False
if training:
while True:
input = raw_input('Press enter')
if input != 'quit':
print 'Left: %s, Right: %s' % (uc.get_joint_positions('l'),
uc.get_joint_positions('r'))
print('Head:', uc.get_head_pose())
else:
break
def move_arms_to_front():
left_initial = [
-0.060724865504715675, -0.04400677423433334, 1.571935610022725,
0.008423494421986533, 7.85542236019144, -1.5718169372528075,
47.124129109964365
]
right_initial = [
-0.09320742998704235, 0.001241879925519199, -1.5748812676586466,
#!/usr/bin/env python
#Author: Ariel Anders
from lis_pr2_pkg.uber_controller import Uber
import rospy
import os
run_dance = lambda x: os.system("rosrun lis_pr2_pkg dance_%s.py" % x)
announce = lambda x: os.system(\
'rosrun sound_play say.py "running the %s dance"' % x.replace("_"," "))
rospy.init_node("alldances")
uc = Uber()
uc.lift_torso()
if __name__ == "__main__":
rospy.init_node("alldances")
uc = Uber()
uc.look_forward()
uc.lift_torso()
dances = [
"michael", "zi", "loc", "caelan_clement", "caris_alex", "rohan_ferran"
]
for dance in dances:
# reset arms
uc.move_arm_to_side("l", blocking=False)
uc.move_arm_to_side("r", blocking=True)
#start dances
announce(dance)
run_dance(dance)
#!/usr/bin/env python
#Author: Nishad Gothoskar
from lis_pr2_pkg.uber_controller import Uber
import rospy
import time
rospy.init_node("test_dance")
rospy.loginfo("Nishad Routine")
uc = Uber()
training = False
if training:
while True:
raw_input('Set Position and hit key')
print 'Left: %s, Right: %s' % (uc.get_joint_positions('l'),
uc.get_joint_positions('r'))
print(uc.get_head_pose())
# initialize positions
uc.close_gripper('l')
uc.close_gripper('r')
uc.open_gripper('l')
uc.open_gripper('r')
uc.move_arm_to_side('l')
uc.move_arm_to_side('r')
uc.look_forward()
left_hand_positions = [[
0.09422760233549077, 0.2728012127918493, 0.1659444924115625,
-0.011844405171707262, 1.2417649091338576, -0.09903999188153156,
28.048339235018787
],
#!/usr/bin/env python
#Author: Nishad Gothoskar
from lis_pr2_pkg.uber_controller import Uber
import rospy
import time
rospy.init_node("test_dance")
rospy.loginfo("Nishad Routine")
uc = Uber()
training = False
if training:
while True:
raw_input('Set Position and hit key')
print 'Left: %s, Right: %s'%(uc.get_joint_positions('l'), uc.get_joint_positions('r'))
print (uc.get_head_pose())
# initialize positions
uc.close_gripper('l')
uc.close_gripper('r')
uc.open_gripper('l')
uc.open_gripper('r')
uc.move_arm_to_side('l')
uc.move_arm_to_side('r')
uc.look_forward()
left_hand_positions = [[0.09422760233549077, 0.2728012127918493, 0.1659444924115625, -0.011844405171707262, 1.2417649091338576, -0.09903999188153156, 28.048339235018787],
[0.08601984721714917, -0.5364338216253639, 0.1454190746362174, -0.1877408195026936, 1.2383519310037745, -0.09873542944083502, 28.04803467257809]]
right_hand_positions = [[-0.368789026081663, -0.4699518417662933, 0.05592731652309624, -0.43877323304173077, -7.511689438923872, -0.09765892758541128, 15.317959108114861],
[-0.17926449880359352, 0.348757824038619, 0.054965187564877116, -0.03254661689955185, -7.511400203489119, 0.007328096614910851, 15.309561886535638]]
head_motion = [(0, .4),(-.8, 0),(0, -.4),(.8, 0)]
def listener():
rospy.init_node('pr2_ctrl_listener', anonymous=True)
global uc
uc = Uber()
rospy.Subscriber("pybullet_cmds", uc_msg, callback)
rospy.spin()
#Detect green box!
params = cv2.SimpleBlobDetector_Params()
params.filterByArea = True
params.filterByCircularity = False
params.filterByConvexity = False
params.minArea = 150
params.maxArea = 10e20
lower_blue = np.array([40,100,100])
upper_blue = np.array([80,255,255])
image_cv = self.bridge.imgmsg_to_cv2(data)
blur = cv2.GaussianBlur(image_cv,(41,41),0)
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
im = cv2.inRange(hsv, lower_blue, upper_blue)
im = cv2.bitwise_not(im)
detect = cv2.SimpleBlobDetector(params)
keypoints = detect.detect(im)
im_with_keypoints = cv2.drawKeypoints(im, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
#Head tracks green box, keeping it at coordinates [300, 300]
self.uc.command_head([clamp_head((300 - keypoints[0].pt[0]) / 100), clamp_head(-(300 - keypoints[0].pt[1]) / 100)], time=1, blocking=False)
self.pub.publish(str((300 - keypoints[0].pt[0]) / 100) + " " + str((300 - keypoints[0].pt[1]) / 100))
def clamp_head(n):
return max(-1.5, min(n, 1.5)) #So the head doesn't swing too far
uc = Uber()
uc.command_joint_pose('l', [0]*7, time=2, blocking=True)
uc.command_joint_pose('r', [0]*7, time=2, blocking=True)
uc.look_down_center()
det = Detector()
rospy.spin()
#!/usr/bin/env python
#Author: Zi Wang
from lis_pr2_pkg.uber_controller import Uber
import rospy
import numpy as np
import csv
is_record = 0
rospy.init_node("test_dance")
rospy.loginfo("Start dancing")
uc = Uber()
# initialize positions
uc.close_gripper('l')
uc.close_gripper('r')
uc.move_arm_to_side("l")
uc.move_arm_to_side("r")
uc.look_forward()
posefile = 'zi_poses.csv'
# record positions
joint_poses = []
cnt = 0
while (is_record and cnt < 10) and not rospy.is_shutdown():
joints = []
joints += uc.get_joint_positions('l')
joints += uc.get_joint_positions('r')
#print uc.return_cartesian_pose('l')
#print uc.return_cartesian_pose('r')
joints += uc.get_head_pose()
#!/usr/bin/env python
#Author: Skye Thompson
from lis_pr2_pkg.uber_controller import Uber
import rospy
import time
import numpy as np
rospy.init_node("test_dance")
rospy.loginfo("Skye Routine")
uc = Uber()
# initial
uc.close_gripper('l')
uc.close_gripper('r')
uc.command_joint_pose('l', [0] * 7, time=2, blocking=True)
uc.command_joint_pose('r', [0] * 7, time=2, blocking=True)
uc.look_forward()
for i in range(2):
#Head right, Left arm up, Right arm down
uc.command_head([-1, 0], time=1, blocking=False)
l = [1, 0, 0, -1.5, 0, 0, 0]
uc.command_joint_pose('l', l, time=2, blocking=True)
r = [-1, 0, -np.pi, -1.5, 0, 0, 0]
uc.command_joint_pose('r', r, time=2, blocking=True)
time.sleep(2)
#Flatten and bend left arm
l = [1, 0, 0, 0, 0, 0, 0]
uc.command_joint_pose('l', l, time=2, blocking=True)
raw_input("get cartesian pose-- left")
print uc.return_cartesian_pose('l', 'base_link')
raw_input("get cartesian pose--right")
print uc.return_cartesian_pose('r', 'base_link')
def test_custom_joint():
rospy.loginfo("sending joints to 0!")
uc.command_joint_pose('l', [0]*7, time=2, blocking=False)
uc.command_joint_pose('r', [0]*7, time=2, blocking=True)
"""
rospy.init_node("ubertest")
rospy.loginfo("how to use uber controller")
uc = Uber()
#while True:
# raw_input('Continue?')
# print '(%s, %s),'%(uc.get_joint_positions('l'), uc.get_joint_positions('r'))
#while True:
# raw_input('')
# print str(uc.get_head_pose()) + ','
# print
#quit()
pairs = [
([
1.5050495098993175, 1.164516297235906, 3.2029045490308374,
if len(faces):
rospy.loginfo("Face detected.")
#cv2.imshow('image',image_cv)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
self.pub.publish("Detected %d faces." % len(faces))
#cv2.imshow('image', image_cv)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#cv2.imwrite('/home/demo/tmp.png',image_cv)
rospy.init_node("test_reactive")
rospy.loginfo('start testing reactive controller')
uc = Uber()
uc.lift_torso()
uc.move_arm_to_side('r')
uc.move_arm_to_side('l')
uc.open_gripper('r')
uc.command_head([-np.pi / 3, np.pi / 8], 3, True)
#uc.look_forward()
det = Detector()
speak = lambda x: os.system("rosrun sound_play say.py '%s'" % x)
class Sub:
def __init__(self):
self.sub = rospy.Subscriber("ifdetect", String, self.cb)
self.detected = False
#!/usr/bin/env python
'''
----------------------------------------------
-- move the robot
-- Jingxi Xu
----------------------------------------------
'''
from lis_pr2_pkg.uber_controller import Uber
import rospy
import numpy as np
import time
import csv
rospy.init_node("ubertest")
rospy.loginfo("motion")
uc = Uber()
# initial
uc.look_forward()
uc.move_arm_to_side("l")
uc.move_arm_to_side("r")
def __init__(self):
self.uc = Uber()
self.uc.open_gripper("l")
class LeftGrip:
def __init__(self):
self.uc = Uber()
self.uc.open_gripper("l")
speak("I do not like red")
elif string_msg.data == "green":
speak("yeah baby")
for _ in range(2):
uc.command_joint_pose('l', [1.5, -1, 3, 1, 0, 0, 0], time=1, blocking=True)
uc.command_joint_pose('r', [-1.5, -1,-3, 1, 0, 0, 0], time=1, blocking=True)
time.sleep(1)
uc.command_joint_pose('l', [1.5, 0.3, 3, 0, 0, 0, 0], time=1, blocking=True)
uc.command_joint_pose('r', [-1.5, 0.3,-3, 0, 0, 0, 0], time=1, blocking=True)
time.sleep(1)
else:
speak("show me something")
self.cnt+=1
if self.cnt == 10:
self.subscriber.unregister()
if __name__ == "__main__":
rospy.init_node("corlor_detector")
rospy.loginfo('Node initialized. Yeah, Baby!')
uc = Uber()
uc.command_head([0, 1], time=1, blocking=False)
time.sleep(1)
uc.close_gripper('l')
uc.close_gripper('r')
uc.command_joint_pose('l', [1.5, 0.3, 3, 0, 0, 0, 0], time=1, blocking=True)
uc.command_joint_pose('r', [-1.5, 0.3,-3, 0, 0, 0, 0], time=1, blocking=True)
detector = Detector()
controller = MotionController()
rospy.spin()
for (ex,ey,ew,eh) in eyes:
cv2.rectangle(roi_color,(ex,ey),(ex+ew,ey+eh),(0,255,0),2)
if len(faces):
rospy.loginfo("Face detected.")
#cv2.imshow('image',image_cv)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
self.pub.publish("Detected %d faces." % len(faces))
#cv2.imshow('image', image_cv)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#cv2.imwrite('/home/demo/tmp.png',image_cv)
rospy.init_node("test_reactive")
rospy.loginfo('start testing reactive controller')
uc = Uber()
uc.lift_torso()
uc.move_arm_to_side('r')
uc.move_arm_to_side('l')
uc.open_gripper('r')
uc.command_head([-np.pi/3,np.pi/8],3,True)
#uc.look_forward()
det = Detector()
speak = lambda x: os.system("rosrun sound_play say.py '%s'" % x)
class Sub:
def __init__(self):
self.sub = rospy.Subscriber("ifdetect", String, self.cb)
self.detected = False
#!/usr/bin/env python
#Author: Ivan Jutamulia
from lis_pr2_pkg.uber_controller import Uber
import rospy
import time
rospy.init_node("Dance_test")
rospy.loginfo("Ivan's dance")
uc = Uber()
# finding position parameters
training = False
if training:
while True:
input = raw_input('Press enter')
if input != 'quit':
print 'Left: %s, Right: %s'%(uc.get_joint_positions('l'), uc.get_joint_positions('r'))
print ('Head:', uc.get_head_pose())
else:
break
def move_arms_to_front():
left_initial = [-0.060724865504715675, -0.04400677423433334, 1.571935610022725, 0.008423494421986533, 7.85542236019144, -1.5718169372528075, 47.124129109964365]
right_initial = [-0.09320742998704235, 0.001241879925519199, -1.5748812676586466, -0.0006970603951756971, -1.5713720799708144, -1.570870962157672, 59.69000916464502]
uc.command_joint_pose('l', left_initial, time=2, blocking=True)
uc.command_joint_pose('r', right_initial, time=2, blocking=True)
# initial positions
def initialize():
uc.close_gripper('l')
uc.close_gripper('r')
and pose configurations, various motions,
etc.
-- Jingxi Xu
----------------------------------------------
'''
from lis_pr2_pkg.uber_controller import Uber
import rospy
import numpy as np
import time
import csv
rospy.init_node("ubertest")
rospy.loginfo("Jingxi's dance demo")
uc = Uber()
# initial
uc.close_gripper('l')
uc.close_gripper('r')
uc.command_joint_pose('l', [0]*7, time=2, blocking=True)
uc.command_joint_pose('r', [0]*7, time=2, blocking=True)
uc.look_forward()
# dance 1
for i in range(2):
l = [ 0.95, 0.0, np.pi/2., -2, -np.pi*1.5, 0, np.pi]
r = [ -0.95, 0.0, -np.pi/2., 0, np.pi*1.5, 0, np.pi]
uc.command_head([-1, 0], time=1, blocking=False)
uc.command_joint_pose('l', l, time=1, blocking=True)
uc.command_joint_pose('r', r, time=1, blocking=True)
#!/usr/bin/env python
#Author: Ariel Anders
from lis_pr2_pkg.uber_controller import Uber
import rospy
import os
run_dance = lambda x: os.system("rosrun lis_pr2_pkg dance_%s.py" % x)
announce = lambda x: os.system(\
'rosrun sound_play say.py "running the %s dance"' % x.replace("_"," "))
rospy.init_node("alldances")
uc = Uber()
uc.lift_torso()
if __name__ == "__main__":
rospy.init_node("alldances")
uc = Uber()
uc.look_forward()
uc.lift_torso()
dances = ["michael","zi", "loc","caelan_clement", "caris_alex", "rohan_ferran"]
for dance in dances:
# reset arms
uc.move_arm_to_side("l", blocking=False)
uc.move_arm_to_side("r", blocking = True)
#start dances
ARM_JOINT_NAMES = {
'left': ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint',
'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'],
'right': ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint',
'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'],
}
TORSO_JOINT_NAME = 'torso_lift_joint'
#Setup node
pub = rospy.Publisher('pybullet_cmds', uc_msg, queue_size=0)
rospy.init_node("pybullet_test")
rospy.loginfo("testing pybullet configurations")
#Setup controller
uc = Uber()
uc.command_joint_pose('l', [0]*7, time=2, blocking=True)
#Setup sim
physicsClient = connect(use_gui=True) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())
pr2_start_orientation = p.getQuaternionFromEuler([0,0,0])
pr2_start_pose = [-.80*k,0,0]
pr2 = p.loadURDF("/home/demo/nishad/pddlstream/examples/pybullet/utils/models/pr2_description/pr2.urdf", pr2_start_pose, pr2_start_orientation,
useFixedBase=True, globalScaling = 1 )
#The goal here is to get the arm positions of the actual robot using the uber controller
#and simulate them in pybullet
print str(uc.get_head_pose()) + ','