def talker():
joint1 = radians(90)
pub = rospy.Publisher('/kukaAllegroHand/robot_cmd', JointState)
rospy.init_node('talker', anonymous=True)
r = rospy.Rate(10000) # 10hz
msg = JointState()
msg.position = [0,joint1,0,0,0,0,0]
pub.publish(msg)
r.sleep()
time.sleep(.4)
while True:
for acc in range(600,601):
print '************************* acceleratioin: %d *************************' % acc
# acc = 200
trjFile = '../trj/acc_new_' + str(acc) + '.dat'
trj = np.genfromtxt(trjFile, delimiter="\t")
done = True
i = 1
while not rospy.is_shutdown() and i < len(trj):
print i
msg = JointState()
msg.position = [0,joint1,0,0,0,0,0]
msg.position[0] = radians(trj[i][0])
msg.position[2] = radians(trj[i][0])
msg.position[3] = radians(trj[i][0])
msg.position[4] = radians(trj[i][0])
msg.position[5] = radians(trj[i][0])
msg.position[6] = radians(trj[i][0])
pub.publish(msg)
r.sleep()
time.sleep((trj[i][2] - trj[i-1][2])/10)
i += 1
def motor_pub():
p1 = rospy.Publisher('/gBot/left_wheel/cmd', JointState)
p2 = rospy.Publisher('/gBot/right_wheel/cmd', JointState)
rospy.init_node('motorPub', anonymous=True)
rospy.on_shutdown(motor_stop)
#Message for left_wheel
msg1 = JointState()
msg1.name = ["left_wheel"]
msg1.position = [0.0]
msg1.velocity = [-100]
#Message for right_wheel
msg2 = JointState()
msg2.name = ["right_wheel"]
msg2.position = [0.0]
msg2.velocity = [-100]
while not rospy.is_shutdown():
msg1.header.stamp = rospy.Time.now()
p1.publish(msg1)
msg2.header.stamp = rospy.Time.now()
p2.publish(msg2)
rospy.loginfo('Left Motor velocity')
rospy.loginfo(msg1.velocity)
rospy.loginfo('Right Motor velocity')
rospy.loginfo(msg2.velocity)
rospy.sleep(0.1)
def execute(self, userdata):
# rospy.loginfo('Executing state %s', self.__class__.__name__)
# self.active = True
# self.first_call = True
# # wait for some time to read from the topic
# # TODO: replace with a service call
# rospy.sleep(3)
# userdata.obj_list = self.obj_list
# self.active = False
# return 'succeeded'
rospy.loginfo("Executing state %s", self.__class__.__name__)
self.obj_list = []
i = 1
for view in userdata.view_list:
rospy.loginfo("%i view: set PTU to %s", i, view)
joint_state = JointState()
joint_state.header.frame_id = "tessdaf"
joint_state.name = ["pan", "tilt"]
joint_state.position = [float(view[0]), float(view[1])]
joint_state.velocity = [float(1.0), float(1.0)]
joint_state.effort = [float(1.0), float(1.0)]
self.ptu_cmd.publish(joint_state)
# wait until PTU has finished or point cloud get screwed up
rospy.sleep(2)
rospy.loginfo("%i view: receive from semantic camera", i)
self.active = True
self.first_call = True
# wait for some time to read once from the topic and store it onto self.pointcloud
# TODO: replace with a service call
rospy.sleep(2)
self.active = False
i = i + 1
userdata.obj_list = self.obj_list
# back to original position
joint_state = JointState()
joint_state.header.frame_id = "tessdaf"
joint_state.name = ["pan", "tilt"]
joint_state.position = [float(0.0), float(0.0)]
joint_state.velocity = [float(1.0), float(1.0)]
joint_state.effort = [float(1.0), float(1.0)]
self.ptu_cmd.publish(joint_state)
return "succeeded"
def main(ra):
# Create Pose Publisher and Subscriber
joint_pub= rospy.Publisher('/dvrk/PSM1/set_torque_joint', JointState, queue_size=10)
rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, callback_joint)
rospy.init_node('Talkity_talk_talk',anonymous=True)
rate = rospy.Rate(ra) # 1hz
#hardcode home to zero
joint_msg= JointState()
joint_msg.header = Header()
joint_msg.header.stamp = rospy.Time.now()
#joint_msg.name = ['outer_yaw','o', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
joint_msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer__wrist_pitch','outer_wrist_jaw', 'jaw']
joint_msg.position=[]
joint_msg.velocity = []
joint_msg.effort = []
#Excitation Trajectory Position Coordinates
i=0
q_data1=[-0.2, -0.2, -0.2, -0.2]
q_data2=[-0.086355,-0.095648,-0.10506,-0.1145,-0.12387,-0.13311,-0.14214,-0.15087,-0.15925,-0.16721,-0.17471,-0.18172,-0.18821,-0.19416,-0.19958,-0.20446,-0.20883,-0.21268,-0.21606,-0.21898,-0.22147,-0.22357,-0.2253,-0.22669,-0.22779,-0.22861,-0.22919,-0.22954,-0.2297,-0.22968,-0.2295,-0.22914,-0.22862,-0.22795,-0.22713,-0.22617,-0.22511,-0.22388,-0.22247,-0.22082,-0.21888,-0.21662,-0.214,-0.21095,-0.20744,-0.20342,-0.19885,-0.19368,-0.18788,-0.1814,-0.17424,-0.16635,-0.15775,-0.14843,-0.13843,-0.12778,-0.11651,-0.10471,-0.092438,-0.079795,-0.066886,-0.053804,-0.040655,-0.027542,-0.014584,-0.0018928,0.010422,0.022271,0.033597,0.044314,0.05438,0.063868,0.072648,0.080705,0.088029,0.09461,0.10049,0.10573,0.11037,0.11443,0.11795,0.12097,0.12353,0.12566,0.12736,0.12866,0.12956,0.13007,0.13023,0.13005,0.12955,0.12874,0.12764,0.12629,0.12472,0.12297,0.12106,0.11903,0.11691,0.11471,0.11247,0.11021,0.10793,0.10564,0.10335,0.10109,0.098863,0.096666,0.094485,0.092317,0.09015,0.087973,0.085786,0.083574,0.081318,0.079023,0.076691,0.074343,0.071992,0.069651,0.067341,0.065096,0.06294,0.060911,0.05905,0.057395,0.055962,0.05478,0.053876,0.053265,0.052956,0.052948,0.053206,0.053699,0.054398,0.055243,0.056172,0.057127,0.058038,0.05881,0.059392,0.059703,0.05959,0.059125,0.058272,0.057011,0.055343,0.053232,0.050668,0.047655,0.044237,0.040451,0.036343,0.031955,0.027341,0.022586,0.017764,0.012952,0.0082075,0.0035806 -0.00086291,-0.0050626,-0.0089787,-0.012584,-0.015859,-0.018781,-0.021356,-0.023586,-0.025488,-0.027074,-0.028361,-0.029398,-0.030214,-0.030849,-0.031337,-0.031706,-0.031991,-0.032234,-0.032468,-0.032704,-0.032954,-0.033233,-0.033555,-0.033912,-0.034301,-0.034697,-0.035081,-0.035422,-0.035697,-0.035867,-0.035877,-0.035676,-0.035207,-0.03442,-0.033253,-0.031662,-0.029576,-0.026944,-0.023725,-0.019887,-0.015407,-0.010266,-0.0044619,0.0020043,0.0091073,0.016811,0.025072,0.033839,0.04304,0.052649,0.062524,0.072569,0.082709,0.092857,0.10293,0.11281,0.12243,0.13173,0.14065,0.14913,0.15713,0.16459,0.1715,0.17786,0.18366,0.1889,0.19362,0.19783,0.20158,0.20492,0.2079,0.21056,0.21295,0.21512,0.21715,0.21904,0.22085,0.22259,0.2243,0.22599,0.22766,0.22932,0.23095,0.23255,0.23409,0.23556,0.23692,0.23812,0.23907,0.2398,0.24025,0.24034,0.24004,0.23926,0.23797,0.23611,0.23359,0.23033,0.22627,0.22136,0.21557,0.20886,0.20118,0.19255,0.18296,0.17243,0.16101,0.14864,0.13538,0.12151,0.10701,0.091903,0.07631,0.060357,0.044198,0.02793,0.011667,-0.0044587,-0.020288,-0.035661,-0.050484,-0.064586,-0.07786,-0.090235,-0.10163,-0.11199,-0.12125,-0.12941,-0.13646,-0.14241,-0.14728,-0.15105,-0.15373,-0.15539,-0.15605,-0.15578,-0.15459,-0.15257,-0.14976,-0.14623,-0.1421,-0.13743,-0.1323,-0.12678,-0.12094,-0.11485,-0.10862,-0.10231,-0.095985,-0.089689,-0.083481,-0.077412,-0.071513,-0.065797,-0.060263,-0.054899,-0.04968,-0.044575,-0.039542,-0.034456,-0.029335,-0.024135,-0.018785,-0.013241,-0.0074516,-0.0014066,0.0049231,0.011561,0.018514,0.025767,0.033291,0.041039,0.048948,0.056956,0.06499,0.072953,0.080753,0.088337,0.095636,0.10252,0.10875,0.11438,0.11943,0.12382,0.12749,0.13041,0.13264,0.1342,0.13509,0.13531,0.13486,0.13379,0.13214,0.12997,0.12732,0.12423,0.12075,0.11694,0.11286,0.10857,0.1041,0.099486,0.09475,0.08992,0.085004,0.080014,0.074935,0.069724,0.064326,0.058826,0.053265,0.047541,0.04157,0.035322,0.028833,0.02214,0.015199,0.0079713,0.00039706,-0.007471,-0.015506,-0.023615,-0.031787,-0.039995,-0.048156,-0.056162,-0.063921,-0.071375,-0.078458,-0.0851,-0.091259,-0.096872,-0.10188,-0.10625,-0.10998,-0.1131,-0.11564,-0.11763,-0.11913,-0.1202,-0.12096,-0.12153,-0.122,-0.12247,-0.12303,-0.12377,-0.12477,-0.12608,-0.1278,-0.1299,-0.13235,-0.13516,-0.13832,-0.14178,-0.14548,-0.14934,-0.15332,-0.15736,-0.16139,-0.16534,-0.16914,-0.17275,-0.17613,-0.1793,-0.18218,-0.18474,-0.18697,-0.18885,-0.19036,-0.19152,-0.19237,-0.19292,-0.19317,-0.19312,-0.19282,-0.19229,-0.19154,-0.19058,-0.18941,-0.18806,-0.18664,-0.18523,-0.18373,-0.18213,-0.18048,-0.17888,-0.17739,-0.17596,-0.17458,-0.17322,-0.17194,-0.17084,-0.16996,-0.16924,-0.16864,-0.16818,-0.16787,-0.16771,-0.16764,-0.16761,-0.1676,-0.16757,-0.1675,-0.16736,-0.16713,-0.16679,-0.1664,-0.16587,-0.16524,-0.16451,-0.1637,-0.16282,-0.16188,-0.16092,-0.15998,-0.15907,-0.15823,-0.15746,-0.15675,-0.1561,-0.15549,-0.15491,-0.15431,-0.15365,-0.15287,-0.15191,-0.15069,-0.14918,-0.14729,-0.14498,-0.14218,-0.13885,-0.13497,-0.13052,-0.12547,-0.11987,-0.11377,-0.10721,-0.10028,-0.093075,-0.085649]
q_data3=[-0.079852,-0.082139,-0.083106,-0.082646,-0.080594,-0.076762,-0.071134,-0.063814,-0.054815,-0.044135,-0.031832,-0.01809,-0.0031553,0.012866,0.029824,0.047487,0.065601,0.083926,0.10234,0.12065,0.1387,0.15626,0.17319,0.18942,0.20498,0.21978,0.23366,0.24665,0.2588,0.27022,0.28092,0.29103,0.30017,0.30862,0.31667,0.32424,0.33127,0.33778,0.34374,0.34927,0.35453,0.35941,0.36388,0.368,0.37184,0.37538,0.37857,0.38148,0.38422,0.38678,0.38903,0.39112,0.39306,0.39486,0.39652,0.39814,0.39971,0.40112,0.40243,0.40374,0.40498,0.40614,0.40727,0.40825,0.40922,0.41017,0.41094,0.41154,0.41218,0.41281,0.41336,0.4141,0.41439,0.4145,0.41497,0.41573,0.41643,0.41699,0.41767,0.41851,0.41953,0.42069,0.42196,0.42346,0.42504,0.42669,0.42843,0.43035,0.43243,0.43441,0.43623,0.43786,0.43943,0.44093,0.44214,0.44285,0.44304,0.44289,0.4424,0.44132,0.43955,0.43711,0.43405,0.4307,0.42677,0.4221,0.41688,0.41125,0.40524,0.39895,0.39239,0.38549,0.37853,0.37166,0.3646,0.35757,0.35082,0.34449,0.33832,0.33227,0.32652,0.32142,0.31679,0.31253,0.30862,0.30518,0.30219,0.29973,0.29775,0.2961,0.29478,0.29393,0.29349,0.29333,0.29348,0.29395,0.29472,0.29585,0.29723,0.29875,0.30057,0.30268,0.30472,0.30738,0.31034,0.31319,0.31598,0.31898,0.32227,0.32572,0.32922,0.33272,0.33621,0.33977,0.34334,0.34694,0.35048,0.35394,0.35726,0.36057,0.36358,0.36645,0.36937,0.37217,0.37462,0.37681,0.3788,0.38067,0.3824,0.38385,0.38505,0.386,0.38683,0.3876,0.3882,0.38855,0.38878,0.38903,0.38924,0.3894,0.38954,0.38964,0.38973,0.38987,0.38996,0.39024,0.39045,0.39056,0.39054,0.39057,0.39061,0.39054,0.39022,0.38975,0.38923,0.38866,0.38787,0.3868,0.38548,0.38407,0.38257,0.38089,0.37898,0.37685,0.37461,0.37221,0.36953,0.3665,0.36309,0.3591,0.35474,0.3498,0.34393,0.33692,0.32875,0.31955,0.30912,0.29724,0.28371,0.26859,0.25196,0.23391,0.21438,0.19339,0.17107,0.14772,0.12357,0.098775,0.073515,0.04809,0.022717,-0.0019257,-0.025823,-0.04901,-0.071158,-0.091765,-0.11066,-0.12781,-0.14327,-0.157,-0.16883,-0.17863,-0.18654,-0.19287,-0.19781,-0.20126,-0.20332,-0.20431,-0.2044,-0.20379,-0.20265,-0.20106,-0.19905,-0.19676,-0.19444,-0.19206,-0.18958,-0.18694,-0.18415,-0.18108,-0.17764,-0.17373,-0.16921,-0.16387,-0.15758,-0.15023,-0.1417,-0.13161,-0.11999,-0.10746,-0.093137,-0.076579,-0.05832,-0.038796,-0.018042,0.0040629,0.027463,0.051969,0.077223,0.10292,0.12895,0.15479,0.18029,0.20545,0.23013,0.25388,0.27614,0.29682,0.31604,0.33383,0.34993,0.36403,0.37608,0.38615,0.39409,0.40046,0.40509,0.4075,0.40754,0.40552,0.40177,0.39601,0.38801,0.37773,0.36534,0.35088,0.33431,0.31538,0.29427,0.27131,0.24652,0.2198,0.19133,0.16153,0.13065,0.098926,0.066691,0.034325,0.0020559,-0.029857,-0.060853,-0.090528,-0.11874,-0.14525,-0.16976,-0.19203,-0.21181,-0.22901,-0.24362,-0.25566,-0.26511,-0.27201,-0.27647,-0.2787,-0.27896,-0.27737,-0.27409,-0.26965,-0.26386,-0.25687,-0.24871,-0.2406,-0.23276,-0.22456,-0.21582,-0.20693,-0.19843,-0.19045,-0.18292,-0.17577,-0.16904,-0.16281,-0.15715,-0.15211,-0.14776,-0.14411,-0.14096,-0.13819,-0.13587,-0.13402,-0.13255,-0.13125,-0.12989,-0.12842,-0.12654,-0.1239,-0.12097,-0.11708,-0.11161,-0.10524,-0.098217,-0.089336,-0.077418,-0.063179,-0.04798,-0.031811,-0.01369,0.0068895,0.029129,0.052422,0.076567,0.1018,0.12787,0.15402,0.17991,0.20572,0.23111,0.25527,0.27779,0.29889,0.3186,0.33655,0.35244,0.36616,0.37776,0.3874,0.39513,0.40098,0.40494,0.40711,0.40757,0.40651,0.40409,0.40054,0.39578,0.38979,0.38254,0.37412,0.36496,0.35435,0.34238,0.32931,0.3152,0.29976,0.28285,0.26448,0.24487,0.22398,0.20186,0.17857,0.15437,0.12942,0.10371,0.076993,0.050524,0.024549,-0.0014408,-0.027645,-0.05315,-0.077226,-0.10005,-0.12186,-0.14242,-0.16106,-0.17779,-0.19311,-0.20692,-0.21881,-0.22894,-0.23753,-0.24408,-0.24874,-0.25271,-0.25664,-0.2598,-0.26109,-0.26095,-0.26058,-0.26071,-0.26048,-0.25967,-0.25854,-0.2577,-0.25711,-0.25636,-0.2554,-0.25472,-0.25436,-0.25387,-0.25307,-0.25219,-0.2515,-0.25087,-0.25005,-0.24893,-0.24764,-0.24649,-0.24529,-0.24406,-0.24276,-0.24139,-0.23999,-0.2387,-0.23762,-0.23689,-0.23632,-0.23591,-0.23565,-0.23566,-0.23601,-0.23656,-0.23719,-0.23776,-0.2382,-0.23847,-0.23848,-0.23807,-0.23715,-0.2354,-0.2328,-0.22933,-0.225,-0.21972,-0.21329,-0.20536,-0.19699,-0.18807,-0.17793,-0.16645,-0.15449,-0.14255]
q_output=[None]*len(q_data1)
qd_output=[None]*len(q_data1)
torque_output=[None]*len(q_data1)
while not rospy.is_shutdown():
if i>len(q_data1)-1:
i=0;
joint_msg.position = []
joint_msg.velocity = []
joint_msg.effort=[q_data1[i],0,0,0,0,0,0]
#joint_msg.effort = [joint_sub.effort[0],joint_sub.effort[1],joint_sub.effort[2],joint_sub.effort[3],joint_sub.effort[4],joint_sub.effort[5],joint_sub.effort[6],joint_sub.effort[7]]
print(joint_msg)
joint_pub.publish(joint_msg)
q_output[i]=joint_sub.position
qd_output[i]=joint_sub.velocity
torque_output[i]=joint_sub.effort
rospy.sleep(1/float(ra))
i+=1
rate.sleep()
#Print Code
with open('data_position.csv', 'wb') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
for i in range(0,len(q_data1)):
wr.writerow(q_output[i])
with open('data_velocity.csv', 'wb') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
for i in range(0,len(q_data1)):
wr.writerow(qd_output[i])
with open('data_torque.csv', 'wb') as myfile:
wr = csv.writer(myfile, quoting=csv.QUOTE_NONE)
for i in range(0,len(q_data1)):
wr.writerow(torque_output[i])
def calibration(portName1, simulated):
print "INITIALIZING TORSO CALIBRATION..."
rospy.init_node("torso")
jointStates = JointState()
jointStates.name = ["spine_connect","waist_connect","shoulders_connect", "shoulders_left_connect", "shoulders_right_connect"]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose",Float32MultiArray, queue_size = 1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size = 1)
msgCurrentPose = Float32MultiArray()
msgCurrentPose.data = [0,0,0]
rate = rospy.Rate(30)
if not simulated:
print "Torso.-> Trying to open serial port on \"" + portName1 + "\""
Roboclaw1.Open(portName1, 38400) #ttyACM0 --- M1: front --- M2: rear
address = 0x80
print "Torso.-> Serial port openned on \"" + portName1 + "\" at 38400 bps (Y)"
print "Torso.-> Clearing previous encoders readings"
a, bumper , b = Roboclaw1.ReadEncM1(address)
print "Torso.-> bumper ", bumper
Roboclaw1.BackwardM2(address, 127) #Abajo-
while bumper == 0 or bumper==1:
a, bumper , b = Roboclaw1.ReadEncM1(address)
print "Torso.->bumper ", bumper
print "Torso.->bumper ", bumper
Roboclaw1.BackwardM2(address, 0)
torsoPos = 0
Roboclaw1.SetEncM2(address, torsoPos)
Roboclaw1.WriteNVM(address)
Roboclaw1.ForwardM2(address, 127) #Arriba+
a, torsoPos , b = Roboclaw1.ReadEncM2(address)
a, bumper , b = Roboclaw1.ReadEncM1(address)
while torsoPos < 84866 or bumper==-1:
a, torsoPos , b = Roboclaw1.ReadEncM2(address)
a, bumper , b = Roboclaw1.ReadEncM1(address)
print "Torso.->bumper ", bumper
Roboclaw1.BackwardM2(address, 0)
a, torsoPos , b = Roboclaw1.ReadEncM2(address)
#print "Torso.-> MAX Torso Pos: ", torsoPos
Roboclaw1.SetEncM2(address, torsoPos)
Roboclaw1.WriteNVM(address)
Roboclaw1.Close()
else:
torsoPos = 84866;
msgCurrentPose.data[0] = (torsoPos*0.352)/169733 #-------------------pulsos a metros
msgCurrentPose.data[1] = 0.0
msgCurrentPose.data[2] = 0.0
pubTorsoPos.publish(msgCurrentPose)
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [(torsoPos*0.352)/169733, 0.0, 0.0, 0.0, 0.0]
pubJointStates.publish(jointStates)
def initialize_psms(self):
if self.__AUTOCAMERA_MODE__ == self.MODE.simulation :
msg = JointState()
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
msg.position = [0.84 , -0.65, 0.10, 0.00, 0.00, 0.00, 0.00]
self.psm1_pub.publish(msg)
msg.position = [-0.84 , -0.53, 0.10, 0.00, 0.00, 0.00, 0.00]
self.psm2_pub.publish(msg)
self.logerror('psms initialized!')
def rotatehead(x):
pub = rospy.Publisher('/head/commanded_state', JointState)
head_command = JointState()
head_command.name=["HeadPan"]
if x > 0:
head_command.position=[0.0] #forwards
else:
head_command.position=[-180.0] #backwards
pub.publish(head_command)
def execute(self, goal):
pickle.dump(goal, open("trajecoryGoalMsgVrep.data", "wb"))
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
num_points = len(trajectory_points)
rospy.loginfo("Received %i trajectory points" % num_points)
pprint(goal.trajectory)
end_time = trajectory_points[-1].time_from_start.to_sec()
control_rate = rospy.Rate(self._control_rate)
i=num_points
rospy.loginfo("the num of point is %i" % i )
#self.jointTrajectoryPublisher.publish(goal.trajectory)
for trajectory_point in trajectory_points:
#Publish for vrepp
jsVrep = JointState()
jsVrep.name = copy.deepcopy(joint_names)
jsVrep.position = trajectory_point.positions
for j in range(len(joint_names)):
jsVrep.name[j] = self.vrep_joint_names[joint_names[j]]
vrep_joint_index = int(jsVrep.name[j][-1]) - 1
print vrep_joint_index
if vrep_joint_index == 4:
self.jointStatePublisherForVrep[vrep_joint_index].publish((-1*jsVrep.position[j]) + self.vrep_position_offset[vrep_joint_index])
else:
self.jointStatePublisherForVrep[vrep_joint_index].publish(jsVrep.position[j] + self.vrep_position_offset[vrep_joint_index])
#Publisher for real robot
js = JointState()
#rospy.loginfo("the length of js.name is %i" %len(js.name))
js.name=copy.deepcopy(joint_names)
for j in range(len(joint_names)):
#rospy.loginfo("j is %i" % j)
js.name[j] = js.name[j]+'_'+str(i)
i=i-1
js.position = trajectory_point.positions
self.jointStatePublisher.publish(js)
control_rate.sleep()
start_time = rospy.get_time()
now_from_start = rospy.get_time() - start_time
last_idx = -1
self._result.error_code = self._result.SUCCESSFUL
self.server.set_succeeded(self._result)
rospy.sleep(5)
self.execution.publish("OK")
def __on_packet(self, buf):
global last_joint_states, last_joint_states_lock
now = rospy.get_rostime()
#stateRT = RobotStateRT.unpack(buf)
stateRT = RobotStateRT_V32.unpack(buf)
self.last_stateRT = stateRT
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0]*6
pub_joint_states.publish(msg)
with last_joint_states_lock:
last_joint_states = msg
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = now
wrench_msg.wrench.force.x = stateRT.tcp_force[0]
wrench_msg.wrench.force.y = stateRT.tcp_force[1]
wrench_msg.wrench.force.z = stateRT.tcp_force[2]
wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
pub_wrench.publish(wrench_msg)
def publish_position():
'''joint position publisher'''
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = JOINT_NAME_ARRAY
msg.position = joints_pos
pub_js.publish(msg)
def gripper_cb(self, data):
js = JointState()
js.header = data.header
js.name = ["Gripper"]
js.position = [data.current_pos]
js.velocity = [data.velocity]
self.gripper_pub.publish(js)
def command_joint_position(self, desired_pose):
"""
Command a specific desired hand pose.
The desired pose must be the correct dimensionality (self._num_joints).
Only the pose is commanded, and **no bound-checking happens here**:
any commanded pose must be valid or Bad Things May Happen. (Generally,
values between 0.0 and 1.5 are fine, but use this at your own risk.)
:param desired_pose: The desired joint configurations.
:return: True if pose is published, False otherwise.
"""
# Check that the desired pose can have len() applied to it, and that
# the number of dimensions is the same as the number of hand joints.
if (not hasattr(desired_pose, '__len__') or
len(desired_pose) != self._num_joints):
rospy.logwarn('Desired pose must be a {}-d array: got {}.'
.format(self._num_joints, desired_pose))
return False
msg = JointState() # Create and publish
try:
msg.position = desired_pose
self.pub_joint.publish(msg)
rospy.logdebug('Published desired pose.')
return True
except rospy.exceptions.ROSSerializationException:
rospy.logwarn('Incorrect type for desired pose: {}.'.format(
desired_pose))
return False
def joint_trajectory_to_joint_state(self, joint_trajectory_msg, index):
joint_states_msg = JointState()
joint_states_msg.name = joint_trajectory_msg.joint_names
joint_states_msg.position = joint_trajectory_msg.points[index].positions
joint_states_msg.velocity = joint_trajectory_msg.points[index].velocities
joint_states_msg.effort = joint_trajectory_msg.points[index].effort
return joint_states_msg
def rc_talker():
'''init pyxhook'''
#Create hookmanager
hookman = pyxhook.HookManager()
#Define our callback to fire when a key is pressed down
hookman.KeyDown = key_down_event
hookman.KeyUp = key_up_event
#Hook the keyboard
hookman.HookKeyboard()
#Start our listener
hookman.start()
pub = rospy.Publisher('rc_sender', String, queue_size=10)
# 2016-10-26 test urdf joint control
# controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch
#roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True
pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10)
rospy.init_node('rc_talker')
rate = rospy.Rate(50)
while not rospy.is_shutdown():
#hello_str = "hello world %s" % rospy.get_time()
pub.publish(''.join(rc_keys))
# 2016-10-26 test urdf joint control
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ['base_to_left_arm', 'base_to_right_arm']
msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])]
pub_2.publish(msg)
#print rc_keys
rate.sleep()
if running == False:
#Close the listener when we are done
hookman.cancel()
break
def test_fk_easy4(self):
chain_state = JointState()
chain_state.position = [0, 0, 0]
eef = self.chain.fk(chain_state, -1)
print eef
eef_expected = numpy.matrix([[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 2], [0, 0, 0, 1]])
self.assertAlmostEqual(numpy.linalg.norm(eef - eef_expected), 0.0, 6)
def computePositionsForNextJoint(self, currPositions, jointsToSweep, maxs, mins, joint_list):
if len(jointsToSweep) != 0:
for kk in range(0, int(1.0/self.resolutionOfSweeps)): # The actual sweeping
currPositions[joint_list.index(jointsToSweep[0])]
currPositions[joint_list.index(jointsToSweep[0])] = mins[joint_list.index(jointsToSweep[0])]+(self.resolutionOfSweeps*kk)*(maxs[joint_list.index(jointsToSweep[0])]-mins[joint_list.index(jointsToSweep[0])])
if (len(jointsToSweep) > 1):
self.computePositionsForNextJoint(currPositions, jointsToSweep[1:len(jointsToSweep)], maxs, mins, joint_list)
# Combine message and publish
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.joint_list
# print self.free_joints
msg.position = currPositions
msg.velocity = [0.0]*len(maxs)
msg.effort = [0.0]*len(maxs)
time.sleep(0.0001)
self.pub.publish(msg)
# Get End Effector Positions
rate = rospy.Rate(20.0)
try:
(trans,rot) = self.listener.lookupTransform(self.baseLink, self.finalLink, rospy.Time(0))
if (self.currX != trans[0]) or (self.currY != trans[1] or self.currZ != trans[2]):
self.csvWriter.writerow(trans)
self.currX = trans[0]
self.currY = trans[1]
self.currZ = trans[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "ERROR: looking up tf"
time.sleep(0.5) # Sleep for a bit to let system catch up
continue
def eyelidServosPut(self,params):
msg = JointState()
msg.name = list()
msg.position = list()
msg.velocity = list()
msg.effort = list()
if params.has_key('leftEyelid_position'):
msg.name.append('left_eyelid_joint')
msg.position.append(math.radians(float(params['leftEyelid_position'])-float(90)))
if params.has_key('leftEyelid_speed'):
#Anadir velocuidad
msg.velocity.append(params['leftEyelid_speed'])
else:
msg.velocity.append(3.0)
if params.has_key('rightEyelid_position'):
msg.name.append('right_eyelid_joint')
msg.position.append(math.radians(float(params['rightEyelid_position'])-float(90)))
if params.has_key('rightEyelid_speed'):
#Anadir velocuidad
msg.velocity.append(params['rightEyelid_speed'])
else:
msg.velocity.append(3.0)
msg.header.stamp = rospy.Time.now()
self.cmd_joints_pub.publish(msg)
def generate_views(self):
rospy.loginfo('Generate views (%s views at pose)' % self.views_at_pose)
try:
resp = self.nav_goals(10, self.inflation_radius, self.roi)
if not resp.goals.poses:
self.views = []
return
for j in range(len(resp.goals.poses)):
for i in range(0,self.views_at_pose):
pose = resp.goals.poses[j]
pan = random.uniform(self.min_pan, self.max_pan)
tilt = random.uniform(self.min_tilt, self.max_tilt)
jointstate = JointState()
jointstate.name = ['pan', 'tilt']
jointstate.position = [pan, tilt]
jointstate.velocity = [self.velocity, self.velocity]
jointstate.effort = [float(1.0), float(1.0)]
resp_ptu_pose = self.ptu_fk(pan,tilt,pose)
p = Pose()
view = ScitosView(self.next_id(), pose,jointstate, resp_ptu_pose.pose)
self.views.append(view)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def _move_to(self, point, dur):
rospy.sleep(dur)
msg = JointState()
with self.lock:
if not self.sig_stop:
self.joint_positions = point.positions[:]
self.joint_velocities = point.velocities[:]
msg.name = ['joint_1',
'joint_2',
'joint_3',
'joint_4',
'joint_5',
'joint_6',
'joint_7']
#msg.name = self.joint_names
msg.position = self.joint_positions
msg.velocity = self.joint_velocities #[0.2]*7
msg.header.stamp = rospy.Time.now()
self.joint_command_pub.publish(msg)
#rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur))
else:
rospy.loginfo('Stoping motion immediately, clearing stop signal')
self.sig_stop = False
def publishJointState(self):
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.jointNames
msg.position = self.getJointAngles()
msg.effort = self.getLoads()
self.jointStatePub.publish(msg)
def joint_state_publisher(self):
try:
joint_state_msg = JointState()
joint_fb_msg = FollowJointTrajectoryFeedback()
time = rospy.Time.now()
with self.lock:
#Joint states
joint_state_msg.header.stamp = time
joint_state_msg.name = self.joint_names
joint_state_msg.position = self.motion_ctrl.get_joint_positions()
#self.joint_state_pub.publish(joint_state_msg)
#Joint feedback
joint_fb_msg.header.stamp = time
joint_fb_msg.joint_names = self.joint_names
joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
#self.joint_feedback_pub.publish(joint_fb_msg)
except Exception as e:
rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def head_wander():
rospy.init_node('head_wander')
joints_pub = rospy.Publisher('/cmd_joints', JointState)
say = rospy.ServiceProxy('/qbo_talk/festival_say', Text2Speach)
seq = 0
while not rospy.is_shutdown():
seq += 1
msg = JointState()
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_footprint'
msg.name = ['head_pan_joint','head_tilt_joint']
msg.position = [random.uniform(-0.9,0.9), random.uniform(-0.8,0.3)]
joints_pub.publish(msg)
if random.random() < 0.3:
try:
say(random.choice(UTTERANCES))
except rospy.ServiceException:
# Ignore errors
pass
rospy.sleep(2.0)
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
def update(self):
if not self.driver_status == 'DISCONNECTED':
# Get Joint Positions
self.current_joint_positions = self.rob.getj()
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "robot_secondary_interface_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0]*6
msg.effort = [0]*6
self.joint_state_publisher.publish(msg)
# Get TCP Position
tcp_angle_axis = self.rob.getl()
# Create Frame from XYZ and Angle Axis
T = PyKDL.Frame()
axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
# Get norm and normalized axis
angle = axis.Normalize()
# Make frame
T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
T.M = PyKDL.Rotation.Rot(axis,angle)
# Create Pose
self.current_tcp_pose = tf_c.toMsg(T)
self.current_tcp_frame = T
self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def loop(self):
hz = 20
r = rospy.Rate(hz)
msg = JointState()
for i in range(8):
msg.name.append("q"+str(i+1))
while not rospy.is_shutdown() and not self.endthread:
br = tf.TransformBroadcaster()
msg.header.stamp=rospy.Time.now()
msg.position = 8 * [0.0]
msg.effort = 8 * [0.0]
self.torque[6]=0.1
for i in range(8):
msg.position[i]=self.q_desired[i]
msg.effort[i]=self.torque[i]
if(self.publishInfo):
self.publisher.publish(msg)
if(self.publishDesiredInfo):
br.sendTransform((self.wTp_desired[0],
self.wTp_desired[1],
self.wTp_desired[2]),
tf.transformations.quaternion_from_euler(self.wTp_desired[3],
self.wTp_desired[4],
self.wTp_desired[5]),
rospy.Time.now(),
self.child_frame,
self.parent_frame)
# Add in the tf transform now
self.master.update_idletasks()
r.sleep()
def set_stiffness(self):
stiffness_message = JointState()
stiffness_message.name = ["HeadYaw", "HeadPitch"]
stiffness_message.position = [0,0]
stiffness_message.velocity = [0,0]
stiffness_message.effort = [1,1]
self.stiffness_publisher.publish(stiffness_message)
def generateJointStatesFromRegisterStateAndPublish():
rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, updateLocalJointState)
global currentJointState
currentJointState = JointState()
currentJointState.header.frame_id = "robotiq_s_model from real-time state data"
currentJointState.name = ["finger_1_joint_1",
"finger_1_joint_2",
"finger_1_joint_3",
"finger_2_joint_1",
"finger_2_joint_2",
"finger_2_joint_3",
"finger_middle_joint_1",
"finger_middle_joint_2",
"finger_middle_joint_3",
"palm_finger_1_joint",
"palm_finger_2_joint"]
currentJointState.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
currentJointState.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
currentJointState.effort = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pub = rospy.Publisher('joint_states', JointState, queue_size=1) #no buffering?
rospy.init_node('robotiq_s_model_joint_state_publisher', anonymous=False)
rate = rospy.Rate(100) # publishes at 100Hz
while not rospy.is_shutdown():
currentJointState.header.stamp = rospy.Time.now()
pub.publish(currentJointState)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def js_cb(self, a):
rospy.loginfo('Received array')
js = JointState()
js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
jList = a.data
jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
i = 0
for pos in jMatrix:
rospy.loginfo(pos)
js.position = pos
gsvr = GetStateValidityRequest()
rs = RobotState()
rs.joint_state = js
gsvr.robot_state = rs
gsvr.group_name = "both_arms"
resp = self.coll_client(gsvr)
if (resp.valid == False):
rospy.loginfo('false')
rospy.loginfo(i)
self.js_pub.publish(0)
return
i = i + 1
self.js_pub.publish(1)
rospy.loginfo('true')
return
def default(self, ci="unused"):
""" Publish the data of the posture sensor as a ROS JointState message """
js = JointState()
js.header = self.get_ros_header()
js.name = [
"kuka_arm_0_joint",
"kuka_arm_1_joint",
"kuka_arm_2_joint",
"kuka_arm_3_joint",
"kuka_arm_4_joint",
"kuka_arm_5_joint",
"kuka_arm_6_joint",
"head_pan_joint",
"head_tilt_joint",
]
js.position = [
self.data["seg0"],
self.data["seg1"],
self.data["seg2"],
self.data["seg3"],
self.data["seg4"],
self.data["seg5"],
self.data["seg6"],
self.data["pan"],
self.data["tilt"],
]
# js.velocity = [1, 1, 1, 1, 1, 1, 1]
# js.effort = [50, 50, 50, 50, 50, 50, 50]
self.publish(js)
def resethead(): # rotate the head(not PTU)
pub = rospy.Publisher('/head/commanded_state', JointState)
head_command = JointState()
head_command.name=["HeadPan"]
head_command.position=[0.0] #forwards
pub.publish(head_command)
eyeplay(0)# reset eyes
def cb_state(msg): if len(msg.motor_states) == 3: state = JointState() state.header.stamp = rospy.Time.now() state.name = ["pan_joint", "tilt_joint", "left_finger_joint", "right_finger_joint"] position_encoder = [msg.motor_states[0].position, msg.motor_states[1].position, msg.motor_states[2].position] position = encoder2rad(position_encoder) gripper_dis = abs(position[2]) * 0.0065 state.position = position[0:2] state.position.append(gripper_dis) state.position.append(gripper_dis) state.velocity = [msg.motor_states[0].speed, msg.motor_states[1].speed, msg.motor_states[2].speed, msg.motor_states[2].speed] pub_joint_state.publish(state)
def __init__(self):
rospy.init_node('moveit_web', disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states', JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world',
PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text': 'ready to plan', 'ready': True}
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(10) # 10hz
joint_msg = JointState()
joint_msg.header = Header()
joint_msg.name = ['head_to_core', 'core_to_left_arm', 'core_to_right_arm']
joint_msg.velocity = []
joint_msg.effort = []
j = 0.01
while not rospy.is_shutdown():
if (j > 3.13):
j = 0.01
else:
j = j + 0.01
joint_msg.header.stamp = rospy.Time.now()
joint_msg.position = [1.57 - j, (1.57 - j), (-1.57 + j)]
rospy.loginfo(joint_msg)
pub.publish(joint_msg)
rate.sleep()
def process_references(self):
# rospy.loginfo("current: {}".format(self.data.position))
# rospy.loginfo("previous: {}".format(self.prev_data.position))
if self.reference_changed(self.data):
self.prev_data.position = self.data.position
safeJointChange = JointState()
safeJointChange.header.seq = 0
safeJointChange.header.stamp.secs = 0
safeJointChange.header.stamp.nsecs = 0
safeJointChange.header.frame_id = ''
safeJointChange.name = ['head_pan_joint', 'head_tilt_joint']
position = [self.data.position[0], self.data.position[1]]
position[0] = min(max(position[0], -3.84), 1.75)
position[1] = min(max(position[1], -1.57), 0.52)
safeJointChange.position = position
safeJointChange.velocity = [0, 0]
safeJointChange.effort = [0, 0]
self.client_safe_joint_change.wait_for_service()
self.client_safe_joint_change(safeJointChange)
rospy.loginfo('Head bridge: Succeeded')
def set_state(self):
joint_state = JointState()
joint_state.name = []
joint_state.position = []
joint_state.velocity = []
joint_state.effort = []
for j in self.joints:
s = getattr(self, j).state()
if s != None:
joint_state.name.append(j)
joint_state.position.append(s.position)
joint_state.velocity.append(s.velocity)
joint_state.effort.append(s.torque)
else:
joint_state.name.append(j)
joint_state.position.append(0)
joint_state.velocity.append(0)
joint_state.effort.append(0)
joint_state.header.stamp = rospy.Time.now()
self.joint_states = joint_state
self._pub_joint_states.publish(joint_state)
def state_publisher():
pub = rospy.Publisher('joint_states', JointState, queue_size=1)
rospy.init_node('state_publisher')
#msg = JointState()
# rate = rospy.Rate(10) #10Hz
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
# position_input = raw_input("Enter joint values in radian")
#
# Add a subroutine here to listen to a path planner
# Add a subroutine here to solve the output of the path planner (Euclidean 3D) to
# configuration space
msg.position = [1.5, 0, 0, 0, 0, 0]
# velocity_input = raw_input( "Enter joint velocity radian/sec" )
# msg.velocity = map( float, velocity_input.split() )
msg.velocity = [0, 0, 0, 0, 0, 0]
#torque_input = input( "Enter the torque in Nm" )
msg.effort = 6 * [0.0]
pub.publish(msg)
def fk_request(self, joint_angles, end_point='right_hand'):
"""
Forward Kinematics request sent to FK Service
@type joint_angles: dict({str:float})
@param joint_angles: the arm's joint positions
@type end_point: string
@param end_point: name of the end point (should be in URDF)
@return: Forward Kinematics response from FK service
"""
fkreq = SolvePositionFKRequest()
# Add desired pose for forward kinematics
joints = JointState()
joints.name = joint_angles.keys()
joints.position = joint_angles.values()
fkreq.configuration.append(joints)
# Request forward kinematics from base to end_point
fkreq.tip_names.append(end_point)
try:
resp = self._fksvc(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("FK Service call failed: %s" % (e, ))
return False
def thruster_ctrl_msg():
global actuator_vel
global left
global right
global Kp
msg = JointState()
msg.header = Header()
msg.name = ['fwd_left', 'fwd_right']
aux = rudder_ctrl()
left = aux + 100.
right = -aux + 100.
if (left < 0):
left = 0
if (right < 0):
right = 0
left *= 0.0025
right *= 0.0025
msg.position = [left, right]
msg.velocity = []
msg.effort = []
return msg
def odom_cb(data):
"""
:type data: Odometry
:return:
"""
global robot_pose
js = JointState()
js.header = data.header
js.name = ['odom_x', 'odom_y', 'odom_t']
js.position = [
data.pose.pose.position.x, data.pose.pose.position.y,
rotation_from_matrix(
quaternion_matrix([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
]))[0]
]
js.velocity = [
data.twist.twist.linear.x, data.twist.twist.linear.y,
data.twist.twist.angular.z
]
js.effort = [0, 0, 0]
js_pub.publish(js)
def _final_joint_state(self, traj):
idxs = [
i for i, n in enumerate(traj.joint_names)
if not n in self._base_joint_names
]
point = traj.points[-1]
joint_state = JointState()
joint_state.header = traj.header
if len(idxs) > 0:
joint_state.name = [traj.joints_names[idx] for idx in idxs]
if len(point.positions) > 0:
joint_state.position = [point.positions[idx] for idx in idxs]
if len(point.velocity) > 0:
joint_state.velocity = [point.velocities[idx] for idx in idxs]
if len(point.effort) > 0:
joint_state.effort = [point.effort[idx] for idx in idxs]
return joint_state
def execute(self):
# Get latest task plan
plan = self.task_q[0].trajectory
for points in plan.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
self.grasp_[0] = self.task_q[0].grasp
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "sia5"
pub_display_msg.trajectory.append(plan)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory)
rospy.loginfo("Start Task")
self.client.send_goal(goal)
self.client.wait_for_result()
# Grasping
if self.grasp_[0] != self.grasp_[1]:
self.executeGrasp(self.grasp_[0])
self.grasp_[1] = self.grasp_[0]
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states',
qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
loop_rate = self.create_rate(30)
# message declarations
joint_state = JointState()
angle = 0.0
delta = pi / 100.0
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = ['base_body_joint']
joint_state.position = [angle]
angle += delta
# send the joint state and transform
self.joint_pub.publish(joint_state)
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def call(self,
joint_positions,
joint_names=['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6'],
tip_names=["right_gripper_tip"]):
"""
Call 'FKService' service
Parameters
----------
joint_positions : list
List of joint angle values (radians) on which to perform forward kinematics.
joint_names : list
List of joint names for the corresponding joint positions.
tip_names : list
List of tip names to which the forward kinematics are calculated.
Returns
-------
: ForwardKinematicsResponse
Returns a ForwardKinematicsResponse built from the SolvePositionFKResponse object returned by the service.
"""
fkreq = SolvePositionFKRequest()
joints = JointState()
joints.name = joint_names
joints.position = joint_positions
# Add desired pose for forward kinematics
fkreq.configuration.append(joints)
# Request forward kinematics from base to "right_hand" link
fkreq.tip_names = tip_names
try:
rospy.wait_for_service(self.ns, 5.0)
resp = self.service(fkreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return ForwardKinematicsResponse(False, None)
def inverse_kinematics(data):
global pub
global arm1len
global arm2len
global base_hight
global arm3max
global arm3min
global pose_old
global pose
msg = JointState()
msg.name = ['joint1', 'joint2',
'joint3'] #zmienione w move.urdf! bo latwiej wpisywac
msg.header.stamp = rospy.get_rostime()
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
if x**2 + y**2 < (arm1len + arm2len)**2 and x**2 + y**2 > (
arm1len - arm2len)**2 and z > arm3min and z < arm3max:
tt = (x**2 + y**2 - arm1len**2 - arm2len**2) / (2 * arm1len * arm2len)
pose[1] = acos(tt)
if abs(pose[1] - pose_old[1]) > abs(pose[1] + pose_old[1]):
pose[1] *= -1
pose[0] = atan2(
-arm2len * sin(pose[1]) * x + (arm1len + arm2len * tt) * y,
(arm1len + arm2len * tt) * x + arm2len * sin(pose[1]) * y)
pose[2] = -z + base_hight
else:
rospy.logerr('Robot unable to rich there!')
msg.position = pose
pub.publish(msg)
pose_old = pose
def setInitState(self):
#This UR5 configuration corresponds to makeRelToolPose(goal_pos=(0.4,0.1,1.15), goal_orient=(90,0,90))
names = [
'elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
positions = [
1.798991006755836, -0.5565362095219593, 0.42343795140628604,
-1.2350940061066096, -2.718235869123541, 0.00156069251509372
]
efforts = [
-5.623666590693509, -45.75583256981657, -0.0009394297664042095,
0.5664154774663301, 0.009129034897339586, 0.00017957266874718547
]
#Define a set_joint function that can be used to reset the robots joints
set_joint = self.rospy.ServiceProxy("/gazebo/set_model_configuration",
SetModelConfiguration)
try:
#Arguments of set_joint function
#['model_name', 'urdf_param_name', 'joint_names', 'joint_positions']
res = set_joint('coro', '', names, positions)
if not res.success:
self.rospy.logwarn(res.status_message)
except self.rospy.ServiceException as e:
self.rospy.logerr("Service call failed: %s" % e)
#Update MoveIt! robot state so it syncs with gazebo's
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = self.rospy.Time.now()
joint_state.name = names
joint_state.position = positions
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state_to_current_state()
def process_return(self, command, data):
"""Process a return data packet from the robot
Args:
command: The type of return
data: The data in the return
"""
if command == self.commands['pos']:
if self.simulate:
position = data
else:
position = self.calc_pos(data)
rospy.logdebug('raw position data: %s', position)
names = []
positions = []
for motor_id in self.available_motor_ids:
raw_position = position[motor_id]
rad_position = (raw_position - int(
self.joint_config[motor_id]['neutral'])) * int(
self.joint_config[motor_id]['inversion'])*2*math.pi/1023
names.append(self.joint_config[motor_id]['name'])
positions.append(rad_position)
new_msg = JointState()
new_msg.name = names
new_msg.position = positions
new_msg.header.stamp = rospy.Time.now()
self.joint_publisher.publish(new_msg)
self.current_positions = position
self.awaiting_pos_resp = False
elif command == self.commands['current']:
# current = self.calc_current(data)
pass
elif command == self.feedback['seq_num']:
data = ord(data)
self.seq_num = data
rospy.loginfo('got seq num: %s', data)
def main():
# starting ROS node and subscribers
rospy.init_node('yumi_simulator', anonymous=True)
pub = rospy.Publisher('/yumi/egm/joint_states', JointState, queue_size=1)
pub_egm_state = rospy.Publisher('/yumi/egm/egm_states', EGMState, queue_size=1)
simulator = Simulator()
#rospy.Subscriber("/joint_velocity", JointState, simulator.callback, queue_size=1)
rospy.Subscriber("/yumi/egm/joint_group_velocity_controller/command", Float64MultiArray, simulator.callback, queue_size=1)
rospy.Subscriber("/sim/grippers", Float64MultiArray, simulator.callbackGripper, queue_size=1)
rate = rospy.Rate(simulator.updateRate)
msg = JointState()
msg_egm_state = EGMState()
msg_channel0 = EGMChannelState()
msg_channel1 = EGMChannelState()
msg_channel0.active = True
msg_channel1.active = True
msg_egm_state.egm_channels = [msg_channel0, msg_channel1]
seq = 1
while not rospy.is_shutdown():
simulator.update()
msg.header.stamp = rospy.Time.now()
msg.header.seq = seq
msg.name = ["yumi_robr_joint_1", "yumi_robr_joint_2", "yumi_robr_joint_3", "yumi_robr_joint_4",\
"yumi_robr_joint_5", "yumi_robr_joint_6", "yumi_robr_joint_7", "yumi_robl_joint_1", "yumi_robl_joint_2", "yumi_robl_joint_3", \
"yumi_robl_joint_4", "yumi_robl_joint_5", "yumi_robl_joint_6", "yumi_robl_joint_7","gripper_r_joint", "gripper_r_joint_m",\
"gripper_l_joint", "gripper_l_joint_m",]
msg.position = simulator.jointState.GetJointPosition().tolist()
pub.publish(msg)
pub_egm_state.publish(msg_egm_state)
rate.sleep()
seq += 1
def obtain_paintingrobot_states(self):
paintingrobot_joints = JointState()
paintingrobot_joints.header.stamp = rospy.Time.now()
paintingrobot_joints.name = []
paintingrobot_joints.position = []
paintingrobot_joints.velocity = []
paintingrobot_joints.effort = []
paintingrobot_joints.name.append('base_joint1')
paintingrobot_joints.position.append(0.0)
paintingrobot_joints.name.append('base_joint2')
paintingrobot_joints.position.append(0.0)
paintingrobot_joints.name.append('mobilebase_joint')
paintingrobot_joints.position.append(0.0)
print("the received angle is:", self.mobile_platform_joints_value[2])
paintingrobot_joints.name.append('rodclimbing_joint1')
paintingrobot_joints.position.append(
self.jackup_mechanism_joints_value[0])
paintingrobot_joints.name.append('rodclimbing_joint2')
paintingrobot_joints.position.append(
self.jackup_mechanism_joints_value[1])
paintingrobot_joints.name.append('shoulder_joint')
paintingrobot_joints.position.append(self.aubo_joints_value[0])
paintingrobot_joints.name.append('upperArm_joint')
paintingrobot_joints.position.append(self.aubo_joints_value[1])
paintingrobot_joints.name.append('foreArm_joint')
paintingrobot_joints.position.append(self.aubo_joints_value[2])
paintingrobot_joints.name.append('wrist1_joint')
paintingrobot_joints.position.append(self.aubo_joints_value[3])
paintingrobot_joints.name.append('wrist2_joint')
paintingrobot_joints.position.append(self.aubo_joints_value[4])
paintingrobot_joints.name.append('wrist3_joint')
paintingrobot_joints.position.append(self.aubo_joints_value[5])
self.paintingrobot_joints_pub.publish(paintingrobot_joints)
def realize_of_principle(goalPoseMsg, jointName, timeOfWay):
global currentState
q = []
time = []
time.append(0)
i = 0
k = 0
start = np.array(currentState.position[0:countOfJoint - 1])
end = np.array(goalPoseMsg)
const = (end - start)
q.append(start)
while (i <= timeOfWay):
i += iter_time
if (i < timeOfWay / 3):
vel = 9 * i * const / (2 * timeOfWay**2)
elif (i > 2 * timeOfWay / 3):
vel = (-9 / (2 * timeOfWay**2) * i + 9 / (2 * timeOfWay)) * const
else:
vel = 3 / (2 * timeOfWay) * const
q.append(q[k] + vel * iter_time)
k += 1
time.append(i)
q_command = []
for i in range(countOfJoint - 1):
qi = [r[i] for r in q]
q_command.append(qi)
q = q_command
jointMsg = JointState()
jointMsg.name = jointName
for i in range(len(q_command[0])):
jointMsg.position = [q[0][i], q[1][i], q[2][i], q[3][i], gripper]
jointMsg.header.stamp = rospy.Time.now()
jointPub.publish(jointMsg)
rospy.sleep(iter_time)
currentState.position = goalPoseMsg
def joint_states_cb(self, msg):
cmd_msg = JointState()
cmd_msg.header.stamp = rospy.Time.now()
cmd_msg.header.frame_id = 'base_link'
cmd_msg.name = list(msg.name)
cmd_msg.position = list(msg.position)
# Convert the value from linkage_tr to EL
SE = msg.position[msg.name.index('SE')]
TR = msg.position[msg.name.index('linkage_tr')]
EL = TR + SE + math.pi / 2.0 - self.el_offset
cmd_msg.position[msg.name.index('linkage_tr')] = EL
cmd_msg.name[msg.name.index('linkage_tr')] = 'EL'
# Add gripper command
cmd_msg.name.append('gripper')
cmd_msg.position.append(self.gripper_cmd)
# FIXME
WY = msg.position[msg.name.index('WY')]
cmd_msg.position[msg.name.index('WY')] = -WY
# Serialize cmd_msg
file_str = StringIO()
cmd_msg.serialize(file_str)
# Send over udp the cmd_msg sensor_msgs/JointState
self.write_socket.sendto(file_str.getvalue(),
(self.write_ip, self.write_port))
def loop(self):
rate = rospy.Rate(1)
start_time = rospy.Time.now().to_sec()
prev_t = 0
t = 0
while not rospy.is_shutdown():
# t = rospy.Time.now().to_sec() - start_time
dt = t - prev_t
prev_t = t
self.cur_t = t
# print(t)
q = self.getQ(t)
(x, y, z), qtn, rpy, h = self.ks.forward(q)
self.log_file_xyz.write("{} {} {}\n".format(x, y, z))
# safe zone: cylinder, plane of floor
R, z0 = 0.1, 0.4
x0, y0 = 0, 0
if (((x - x0)**2 + (y - y0)**2 > R**2) or (z > z0)) and (z > 0.1):
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = [
'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4',
'arm_joint_5', 'gripper_finger_joint_l',
'gripper_finger_joint_r'
]
msg.position = [0.01] * 7
msg.position[:5] = q
self.js_pub.publish(msg)
else:
print("COLLISION: " + str(t))
t = t + 0.01
rate.sleep()
def talker(joint_handlers_provider):
"""
spam joint states non-stop
"""
if callable(joint_handlers_provider):
handlers = joint_handlers_provider()
else:
handlers = joint_handlers_provider
joint_names = [h.name for h in handlers]
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('scara_joint_state_publisher')
rate = rospy.Rate(10) # 10hz
hello_str = JointState()
hello_str.header = Header()
while not rospy.is_shutdown():
hello_str.header.stamp = rospy.Time.now()
hello_str.name = joint_names
hello_str.position = [next(h) for h in handlers]
hello_str.velocity = []
hello_str.effort = []
pub.publish(hello_str)
rate.sleep()
def publish_joint_state(self):
for id_human, publisher in enumerate(self.joint_state_publishers):
joint_state = JointState()
joint_state.header.stamp = rospy.Time().now()
joint_state.name = [""] * len(self.joints)
joint_state.position = [0.] * len(self.joints)
for id_dof, joint in enumerate(self.joints):
joint_state.name[id_dof] = joint
# TODO REMOVE THAT SHOULDER HACK
# Should work on left arm model to avoid setting
# the shoulder in the parser
# if joint == "lShoulderX":
# joint_state.position[id_dof] = -pi / 2.
if joint in self.active_dofs:
i = self.active_dofs.index(joint)
joint_state.position[id_dof] = self.q_cur[id_human][i]
# Publish joint states
publisher.publish(joint_state)
def plan_trajectory(move_group, destination_pose, start_joint_angles):
print(destination_pose)
current_joint_state = JointState()
current_joint_state.name = joint_names
current_joint_state.position = start_joint_angles
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = current_joint_state
move_group.set_start_state(moveit_robot_state)
move_group.set_pose_target(destination_pose)
plan = move_group.plan()
if not plan:
exception_str = """
Trajectory could not be planned for a destination of {} with starting joint angles {}.
Please make sure target and destination are reachable by the robot.
""".format(destination_pose, destination_pose)
raise Exception(exception_str)
return planCompat(plan)
def talker():
rospy.loginfo("start ...")
rospy.init_node("real_listener", anonymous=True)
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
rate = rospy.Rate(30) # 30hz
# pub joint state
joint_state_send = JointState()
joint_state_send.header = Header()
joint_state_send.name = [
"arm1_joint",
"arm2_joint",
"arm3_joint",
"arm4_joint",
"arm5_joint",
"arm6_joint",
]
joint_state_send.velocity = [0]
joint_state_send.effort = []
rospy.loginfo("start loop ...")
while not rospy.is_shutdown():
radians_list = mc.get_angles()
radians_list = radians_list
rospy.loginfo("res: {}".format(radians_list))
# publish angles.
joint_state_send.header.stamp = rospy.Time.now()
joint_state_send.position = radians_list
pub.publish(joint_state_send)
rate.sleep()
def publish_js():
try:
v = Vector3Stamped()
v.header.frame_id = 'map'
v.vector.z = -1
v = transform_vector('gripper_tool_frame', v)
g = np.array([v.vector.x, v.vector.y, v.vector.z])
g = g / np.linalg.norm(g)
goal_z = np.cross(g, [1, 0, 0])
goal_z = goal_z / np.linalg.norm(goal_z)
angle = np.arccos(np.array([0, 0, 1]).dot(goal_z))
js = JointState()
js.header = v.header
js.name = ['refills_finger_joint']
if goal_z[1] > 0:
angle = -angle
js.position = [angle]
js.velocity = [0]
js.effort = [0]
pub.publish(js)
except Exception as e:
rospy.logwarn(
'failed to published refills finger joint state: {}'.format(e))
def __init__(self):
rospy.init_node('manipulator')
rospy.loginfo("Press Ctrl + C to terminate")
self.rate = rospy.Rate(10)
self.joint_pub = rospy.Publisher('/rx150/joint_states', JointState, queue_size=10)
# prepare joint message to publish
joint_msg = JointState()
joint_msg.header = Header()
joint_msg.name = ['waist', 'shoulder', 'elbow', 'wrist_angle',\
'wrist_rotate', 'gripper', 'left_finger', 'right_finger']
joint_msg.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.026, -0.026]
# test case for inverse kinematics
test_case = [0.02165, 0.01250, 0.29721] # position (x, y, z) in meter
joint_angle = inverse_kinematics(test_case)
print(test_case)
print(joint_angle)
while not rospy.is_shutdown():
joint_msg.header.stamp = rospy.Time.now()
joint_msg.position[0:3] = joint_angle
self.joint_pub.publish(joint_msg)
self.rate.sleep()
def plan_to_config(self, end_state):
"""
Uses MoveIt to plan a path from the current state to end_state and returns it
end_state: list of 6 joint values
"""
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
# the robot has a dumb base_link joint that we don't want
joint_state.name = self.group.get_joints()[1:-1]
print joint_state.name
joint_state.position = end_state
self.group.set_start_state_to_current_state()
try:
self.group.set_joint_value_target(joint_state)
except moveit_commander.MoveItCommanderException:
pass
# Plan the path
plan = self.group.plan()
return plan
def trac_ik_request(self, pose):
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ik_req = GetConstrainedPositionIKRequest()
ik_req.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
current_joint_angles = self._limb.joint_angles()
# get current angles
joint_names = [
self._limb_name + "_s0", self._limb_name + "_s1",
self._limb_name + "_e0", self._limb_name + "_e1",
self._limb_name + "_w0", self._limb_name + "_w1",
self._limb_name + "_w2"
]
current_joint_states = [
current_joint_angles[self._limb_name + "_s0"],
current_joint_angles[self._limb_name + "_s1"],
current_joint_angles[self._limb_name + "_e0"],
current_joint_angles[self._limb_name + "_e1"],
current_joint_angles[self._limb_name + "_w0"],
current_joint_angles[self._limb_name + "_w1"],
current_joint_angles[self._limb_name + "_w2"]
]
req_joint_state = JointState()
req_joint_state.name = joint_names
req_joint_state.position = current_joint_states
# populate the request message
ik_req.seed_angles.append(req_joint_state)
ik_req.num_steps = 1
#ik_req.end_tolerance = 0.0001
try:
resp = self._trac_iksvc(ik_req)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
def fake_mir_joint_publisher():
rospy.init_node('fake_mir_joint_publisher')
prefix = rospy.get_param('~prefix', '')
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
r = rospy.Rate(50.0)
while not rospy.is_shutdown():
js = JointState()
js.header.stamp = rospy.Time.now()
js.name = [
prefix + 'left_wheel_joint', prefix + 'right_wheel_joint',
prefix + 'fl_caster_rotation_joint',
prefix + 'fl_caster_wheel_joint',
prefix + 'fr_caster_rotation_joint',
prefix + 'fr_caster_wheel_joint',
prefix + 'bl_caster_rotation_joint',
prefix + 'bl_caster_wheel_joint',
prefix + 'br_caster_rotation_joint',
prefix + 'br_caster_wheel_joint'
]
js.position = [0.0 for _ in js.name]
js.velocity = [0.0 for _ in js.name]
js.effort = [0.0 for _ in js.name]
pub.publish(js)
r.sleep()
