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 sim_robot():
rospy.init_node("simulated_robot")
pub = rospy.Publisher("joint_states", JointState, queue_size=5)
rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))
msg = JointState()
msg.header = Header(stamp=rospy.Time.now())
msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"]
msg.position = [0] * 6
msg.velocity = [0, 0, 0, 0, 0, 0]
msg.effort = [0, 0, 0, 0, 0, 0]
def joint_cb(msg_in, id):
msg.header = Header(stamp=rospy.Time.now())
msg.position[id] = msg_in.data
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
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 command_joint_torques(self, desired_torques):
"""
Command a desired torque for each joint.
The desired torque must be the correct dimensionality
(self._num_joints). Similarly to poses, we do not sanity-check
the inputs. As a rule of thumb, values between +- 0.5 are fine.
:param desired_torques: The desired joint torques.
:return: True if message is published, False otherwise.
"""
# Check that the desired torque vector can have len() applied to it,
# and that the number of dimensions is the same as the number of
# joints. This prevents passing singletons or incorrectly-shaped lists
# to the message creation (which does no checking).
if (not hasattr(desired_torques, '__len__') or
len(desired_torques) != self._num_joints):
rospy.logwarn('Desired torques must be a {}-d array: got {}.'
.format(self._num_joints, desired_torques))
return False
msg = JointState() # Create and publish
try:
msg.effort = desired_torques
self.pub_joint.publish(msg)
rospy.logdebug('Published desired torques.')
return True
except rospy.exceptions.ROSSerializationException:
rospy.logwarn('Incorrect type for desired torques: {}.'.format(
desired_torques))
return False
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 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 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 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 __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 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 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 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 get_new_JointState(pan, tilt): j = JointState() j.header.stamp = rospy.Time.now() j.name = ["panservo", "tiltservo"] j.position = [pan, tilt] j.velocity = [] j.effort = [] return j
def publishJointStates(self):
jState = JointState()
jState.header.stamp = rospy.Time.now()
jState.name = self.names
jState.position = self.positions
jState.velocity = self.velocities
jState.effort = self.efforts
self.pub.publish(jState)
def publishJointStates(self):
# rospy.loginfo(self.jointPositions)
jState = JointState()
jState.header.stamp = rospy.Time.now()
jState.name = self.jointNamesFromConfig
jState.position = self.jointPositions
jState.velocity = self.jointVelocities
jState.effort = self.jointAccelerations
self.joint_state_pub.publish(jState)
def talker():
global joy_value
global last_joy
global joint_state
rospy.init_node('vsv_arm_teleop_raw')
sub = rospy.Subscriber('~joy', Joy, joy_cb)
jsub = rospy.Subscriber('~joint_state', JointState, joint_cb)
joint = rospy.Publisher('~joint_command', JointState)
axis={}
axis["ArmPan"] = [int(s) for s in str(rospy.get_param("~axis_pan","6")).split(",")]
axis["ArmTilt"] = [int(s) for s in str(rospy.get_param("~axis_tilt","7")).split(",")]
axis["ArmFold"] = [int(s) for s in str(rospy.get_param("~axis_fold","4,5")).split(",")]
axis["ArmExtend"] = [int(s) for s in str(rospy.get_param("~axis_extend","0,3")).split(",")]
axis["ToolRotate"] = [int(s) for s in str(rospy.get_param("~axis_rotate","2,1")).split(",")]
step_value = {"ArmPan":1*pi/180., "ArmTilt":5*pi/180.,
"ArmFold":2*pi/180., "ArmExtend":0.05, "ToolRotate":5*pi/180.}
timeout = True
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not joint_state:
rate.sleep()
if rospy.is_shutdown():
return
command = JointState()
command.header = joint_state.header
command.name = joint_state.name
command.velocity = [0.0] * len(joint_state.name)
command.effort = [0.0] * len(joint_state.name)
command.position = [x for x in joint_state.position]
index = dict(zip(command.name,range(0,len(command.name))))
while not rospy.is_shutdown():
if (rospy.rostime.get_time() - last_joy) < 0.5:
if timeout:
timeout = False
rospy.loginfo("Accepting joystick commands")
for k in axis.keys():
if len(axis[k])==1:
# Axis
command.position[index[k]] = sat(command.position[index[k]]+joy_value.axes[axis[k][0]]*step_value[k],
joint_state.position[index[k]],step_value[k])
else:
# Buttons
if joy_value.buttons[axis[k][0]]:
command.position[index[k]] = sat(command.position[index[k]]-step_value[k],
joint_state.position[index[k]],step_value[k])
elif joy_value.buttons[axis[k][1]]:
command.position[index[k]] = sat(command.position[index[k]]+step_value[k],
joint_state.position[index[k]],step_value[k])
# if twist.linear.x < 0:
# twist.angular.z = - twist.angular.z
else:
if not timeout:
timeout = True
rospy.loginfo("Timeout: ignoring joystick commands")
joint.publish(command)
rate.sleep()
def convertJointTrajectorySensorMsg(self, msg):
# Generate fake JointState msg
sim_data_msg = JointState()
sim_data_msg.header = msg.header
sim_data_msg.name = msg.joint_names
sim_data_msg.position = msg.actual.positions
sim_data_msg.velocity = msg.actual.velocities
sim_data_msg.effort = msg.actual.effort
return sim_data_msg
def run(self):
if(self.initialized):
status = self.gripper.getStatus()
js = JointState()
js.header.stamp = rospy.Time().now()
js.name = ["gripperjoint_1", "gripperjoint_2", "gripperjoint_3"]
js.position = [status.gPOA/255.0, status.gPOB/255.0, status.gPOC/255.0]
js.velocity = [0.0, 0.0, 0.0]
js.effort = [status.gCUA*10, status.gCUB*10, status.gCUC*10]
self.js_pub.publish(js)
def test_command_torques(self):
des_torques = [0.123] * 16
ret = self.client.command_joint_torques(des_torques)
self.assertTrue(ret)
self.assertEqual(1, self.client.pub_joint._pub_count)
published_state = self.client.pub_joint._last_published
ref_state = JointState()
ref_state.effort = des_torques
self.assertEqual(ref_state, published_state)
def loop(self):
hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
# Publish Joint States
while not rospy.is_shutdown():
msg = JointState()
msg.header.stamp = rospy.Time.now()
if delta > 0:
self.update(delta)
# Initialize msg.position, msg.velocity, and msg.effort.
has_position = len(self.joints.items()) > 0
has_velocity = False
has_effort = False
for (name,joint) in self.joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
has_velocity = True
if not has_effort and 'effort' in joint:
has_effort = True
num_joints = len(self.joints.items())
if has_position:
msg.position = num_joints * [0.0]
if has_velocity:
msg.velocity = num_joints * [0.0]
if has_effort:
msg.effort = num_joints * [0.0]
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
# Add Free Joint
if name in self.joints:
joint = self.joints[name]
factor = 1
offset = 0
if has_position and 'position' in joint:
pos = self.joint_positions[ name ]
msg.position[i] = pos
if has_velocity and 'velocity' in joint:
msg.velocity[i] = joint['velocity'] * factor
if has_effort and 'effort' in joint:
msg.effort[i] = joint['effort']
self.pub.publish(msg)
r.sleep()
def execute(self, userdata):
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 point cloud", 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
rospy.loginfo("%i view: call object recognition service", i)
try:
obj_rec_resp = self.obj_rec(self.pointcloud)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
cat_list = obj_rec_resp.categories_found
if len(cat_list) == 0:
rospy.loginfo("%i view: nothing perceived", i)
else:
rospy.loginfo("%i view: found categories: %i", i, len(cat_list))
for cat in cat_list:
rospy.loginfo("Categorie: %s", cat.data)
obj_desc = dict()
obj_desc["type"] = cat.data
# obj_desc['probability'] = 1.0
self.obj_list.append(obj_desc)
i = i + 1
def default(self, ci='unused'):
message = JointState()
message.name = [''] * 7
message.position = [0] * 7
message.velocity = [0] * 7
message.effort = [0] * 7
for i in range(7):
message.name[i] = "arm_%i_joint"%i
message.position[i] = self.data["kuka_%i"%(i+1)]
self.publish(message)
def publishJointStates():
jState = JointState()
jState.header.stamp = rospy.Time.now()
jState.name = jointNames
angles, angvels, myids = dynamixelChain.read_angs_angvels(ids)
global jPositions
jPositions = idValsToJoints(angles)
jState.position = jPositions
jState.velocity = idValsToJoints(angvels)
jState.effort = [0.0] * 10
joint_state_pub.publish(jState)
def JointState(self, ser):
js = JointState()
js.header.seq = ser['seq']
js.header.stamp.secs = ser['secs']
js.header.stamp.nsecs = ser['nsecs']
js.header.frame_id = ser['frame_id']
js.name = ser['name']
js.position = ser['position']
js.velocity = ser['velocity']
js.effort = ser['effort']
return(js)
def obtain_aubo_info(self):
current_pos = self.robot.get_current_waypoint()
current_joint_msg = JointState()
current_joint_msg.header.frame_id = ""
current_joint_msg.header.stamp = rospy.Time.now()
current_joint_msg.position = current_pos['joint']
current_joint_msg.velocity = []
current_joint_msg.effort = []
current_pose_msg = Pose()
current_pose_msg.position.x = current_pos['pos'][0]
current_pose_msg.position.y = current_pos['pos'][1]
current_pose_msg.position.z = current_pos['pos'][2]
current_pose_msg.orientation.x = current_pos['ori'][0]
current_pose_msg.orientation.y = current_pos['ori'][1]
current_pose_msg.orientation.z = current_pos['ori'][2]
current_pose_msg.orientation.w = current_pos['ori'][3]
catersian_velocity = catersian_vel()
time1 = rospy.get_time()
print("time1", time1)
if self.count == 1:
catersian_velocity.vx = 0.0
catersian_velocity.vy = 0.0
catersian_velocity.vz = 0.9
catersian_velocity.v = 0.0
else:
delta_t = time1 - self.array1[3]
print("delta_time is:", delta_t)
catersian_velocity.vx = (current_pose_msg.position.x -
self.array1[0]) / delta_t
catersian_velocity.vy = (current_pose_msg.position.y -
self.array1[1]) / delta_t
catersian_velocity.vz = (current_pose_msg.position.z -
self.array1[2]) / delta_t
catersian_velocity.v = math.sqrt(catersian_velocity.vx**2 +
catersian_velocity.vy**2 +
catersian_velocity.vz**2)
self.count = self.count + 1
print("vx", catersian_velocity.vx)
print("vy", catersian_velocity.vy)
print("vz", catersian_velocity.vz)
print("v_total", catersian_velocity.v)
self.array1[0] = current_pos['pos'][0]
self.array1[1] = current_pos['pos'][1]
self.array1[2] = current_pos['pos'][2]
self.array1[3] = time1
# print("catersian velocity is:",catersian_velocity)
self.aubo_joint_pub.publish(current_joint_msg)
self.aubo_pose_pub.publish(current_pose_msg)
self.aubo_vel_pub.publish(catersian_velocity)
def baxter_play(traj, box_data):
pub1 = rospy.Publisher('/replay/joint_states', JointState, queue_size=10)
rate_value = 200
rate1 = rospy.Rate(rate_value)
pub2 = rospy.Publisher('/stopSim', Float64, queue_size=1)
rate2 = rospy.Rate(160)
prev_pos = traj[0]
for pos in traj:
replay = JointState()
replay.header = Header()
replay.header.stamp = rospy.Time.now()
replay.name = [
'ljoint1', 'ljoint2', 'ljoint3', 'ljoint4', 'ljoint5', 'ljoint6',
'ljoint7', 'rjoint1', 'rjoint2', 'rjoint3', 'rjoint4', 'rjoint5',
'rjoint6', 'rjoint7'
]
replay.position = pos
replay.velocity = (pos - prev_pos) * rate_value
replay.effort = box_data
pub1.publish(replay)
rate1.sleep()
prev_pos = pos
# for vrep to know when a trajectory ends and to keep the joints from moving randomly
for i in range(0, 5):
replay = JointState()
replay.header = Header()
replay.header.stamp = rospy.Time.now()
replay.name = []
replay.position = pos
replay.velocity = (pos - pos
) # to send 0s so that the joints don't go haywire
replay.effort = []
pub1.publish(replay)
pub2.publish(data=1)
rate2.sleep()
def transform_callback(self, data):
px = data.pose.position.x
py = data.pose.position.y
pz = data.pose.position.z
r = quaternion_matrix([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w])
a1 = 3
a2 = 2
#th1 = atan2(py, px)
#th2 = asin(-pz/a)
#th3 = asin(-r[2][0])-th2
#th3_2 = acos(r[0][0]*cos(th1) + r[1][0]*sin(th1)) - th2
th1 = atan(py/px)
k2 = pz
k1 = px*cos(th1) + py*sin(th1)
th2=-2*atan((2*a1*k2 - sqrt(- a1**4 + 2*a1**2*a2**2 + 2*a1**2*k1**2 + 2*a1**2*k2**2 - a2**4 + 2*a2**2*k1**2 + 2*a2**2*k2**2 - k1**4 - 2*k1**2*k2**2 -k2**4))/(a1**2 + 2*a1*k1 - a2**2 + k1**2 + k2**2))
th2_2=-2*atan((2*a1*k2 + sqrt(- a1**4 + 2*a1**2*a2**2 + 2*a1**2*k1**2 + 2*a1**2*k2**2 - a2**4 + 2*a2**2*k1**2 + 2*a2**2*k2**2 - k1**4 - 2*k1**2*k2**2 - k2**4))/(a1**2 + 2*a1*k1 - a2**2 + k1**2 + k2**2))
th3=-2*atan(sqrt((- a1**2 + 2*a1*a2 - a2**2 + k1**2 + k2**2)*(a1**2 + 2*a1*a2 + a2**2 - k1**2 - k2**2))/(- a1**2 + 2*a1*a2 - a2**2 + k1**2 + k2**2))
th3_2=2*atan(sqrt((- a1**2 + 2*a1*a2 - a2**2 + k1**2 + k2**2)*(a1**2 + 2*a1*a2 + a2**2 - k1**2 - k2**2))/(- a1**2 + 2*a1*a2 - a2**2 + k1**2 + k2**2))
print(str(th2)+" | "+str(th2_2))
if abs(self.th2_prev-th2) > abs(self.th2_prev-th2_2) and abs(self.th2_prev-th2)<6.28:
th2 = th2_2
th3 = th3_2
#if abs(th2_2) > abs(th2):
self.th2_prev=th2
self.th3_prev=th3
#if abs(th3_2) > abs(th3):
#print(str(th3)+str(th3_2))
#if req.time <= 0 or not 0 <= req.j1 <= 6 or not -3 <= req.j2 <= 3 or not -1 <= req.j3 <= 1:
# return False
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['1_na_boki', '1_przod_tyl', 'czlon_z_2']
hello_str.position = [th_0[0]+th1, th_0[1]+th2, th_0[2]+th3]
hello_str.velocity = []
hello_str.effort = []
self.pub.publish(hello_str)
return (str(th1)+" "+str(th2)+" "+str(th3))
def _publish_joint_state(self):
js = JointState()
js.header.stamp = rospy.Time.now()
js.header.frame_id = 'cozmo'
js.name = ['head', 'lift']
js.position = [
self._cozmo.head_angle.radians,
self._cozmo.lift_height.distance_mm * 0.001
]
js.velocity = [0.0, 0.0]
js.effort = [0.0, 0.0]
self._joint_state_pub.publish(js)
def state_publisher():
pub = rospy.Publisher('joint_states', JointState, queue_size=1, latch=True)
rospy.init_node('state_publisher')
#msg = JointState()
rate = rospy.Rate(1) #1Hz
position = 6 * [0.0]
velocity = 6 * [0.0]
torque = 6 * [0.0]
## Non-latch start
# while not rospy.is_shutdown():
# msg = JointState()
# msg.header.stamp = rospy.Time.now()
# msg.name = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
# msg.position = position
# msg.velocity = velocity
# msg.effort = torque
# pub.publish(msg)
# position_input = raw_input("Enter joint values in radian: ")
# velocity_input = raw_input( "Enter joint velocity radian/sec: " )
# torque_input = raw_input( "Enter the torque in Nm" )
# position = map( float, position_input.split() )
# velocity = map( float, velocity_input.split() )
# torque = map( float, torque_input.split() )
# position = [ -1.5, -1.5, -1.5, -1.5, -1.5, -1.5 ]
# velocity = [ 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 ]
# rate.sleep()
# Non-latch ends
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
msg.position = position
msg.velocity = velocity
msg.effort = torque
pub.publish(msg)
# position_input = raw_input("Enter joint values in radian: ")
# velocity_input = raw_input( "Enter joint velocity radian/sec: " )
# torque_input = raw_input( "Enter the torque in Nm" )
# position = map( float, position_input.split() )
# velocity = map( float, velocity_input.split() )
# torque = map( float, torque_input.split() )
position = [-1.5, -1.5, -1.5, -1.5, -1.5, -1.5]
velocity = [0.2, 0.2, 0.2, 0.2, 0.2, 0.2]
rate.sleep()
def loop(self):
while not rospy.is_shutdown():
#TODO publish actual gripper state
msg = JointState()
msg.name = ['robotiq_85_left_knuckle_joint']
msg.position = [0]
msg.velocity = [0]
msg.effort = [0]
self._joint_state_pub.publish(msg)
rospy.sleep(self._joint_publish_rate)
def odomcb(self, data):
self.X = self.odom_to_state(data)
# if self.running_flag:
joint_states = JointState()
joint_states.header = Header()
joint_states.header.stamp = rospy.Time.now()
joint_states.name = ['virtual_x', 'virtual_y', 'virtual_theta']
joint_states.position = [self.X[0], self.X[1], self.X[2]]
joint_states.velocity = []
joint_states.effort = []
self.pub.publish(joint_states)
return
def fnc_callback(msg):
hand_pos = JointState()
data = [ord(x) for x in msg.msg]
if data[0] == 241:
#da modificare
right_thumb1 = +1.125 - float(data[6]) / 180 * 1.125
right_thumb = 1.5 - float(data[1]) / 180 * 1.5
right_thumb3 = 1.5 - float(data[1]) / 180 * 1.5
right_index1 = 1.5 - float(data[2]) / 180 * 1.5
right_index = 1.5 - float(data[2]) / 180 * 1.5
right_index3 = 1.5 - float(data[2]) / 180 * 1.5
right_middle1 = 1.5 - float(data[3]) / 180 * 1.5
right_middle = 1.5 - float(data[3]) / 180 * 1.5
right_middle3 = 1.5 - float(data[3]) / 180 * 1.5
#da modificare
right_ring1 = -0.15 + float(data[7]) / 180 * 0.15
right_ring = 1.5 - float(data[4]) / 180 * 1.5
right_ring3 = 1.5 - float(data[4]) / 180 * 1.5
right_ring4 = 1.5 - float(data[4]) / 180 * 1.5
#da modificare
right_pinky1 = -0.15 + float(data[8]) / 180 * 0.15
right_pinky = 1.5 - float(data[5]) / 180 * 1.5
right_pinky3 = 1.5 - float(data[5]) / 180 * 1.5
right_pinky4 = 1.5 - float(data[5]) / 180 * 1.5
base_to_hand = -3.15 + float(data[9]) / 180 * 3.15
hand_pos.name = [
'right_thumb1', 'right_thumb', 'right_thumb3', 'right_index1',
'right_index', 'right_index3', 'right_middle1', 'right_middle',
'right_middle3', 'right_ring1', 'right_ring', 'right_ring3',
'right_ring4', 'right_pinky1', 'right_pinky', 'right_pinky3',
'right_pinky4', 'base_to_hand'
]
hand_pos.position = [
right_thumb1, right_thumb, right_thumb3, right_index1, right_index,
right_index3, right_middle1, right_middle, right_middle3,
right_ring1, right_ring, right_ring3, right_ring4, right_pinky1,
right_pinky, right_pinky3, right_pinky4, base_to_hand
]
hand_pos.velocity = []
hand_pos.effort = []
hand_pos.header = Header()
hand_pos.header.stamp = rospy.Time.now()
rospy.loginfo(hand_pos)
pub.publish(hand_pos)
def send_hand_position(command_pub, hand_joint_list): print "\tSending hand positions: ", hand_joint_list command = JointState() command.header.stamp = rospy.Time.now() command.name = ['bh_j23_joint', 'bh_j12_joint', 'bh_j22_joint', 'bh_j32_joint', 'bh_j33_joint', 'bh_j13_joint', 'bh_j11_joint', 'bh_j21_joint'] command.position = hand_joint_list command.velocity = [0, 0, 0, 0, 0, 0, 0, 0] command.effort = [0, 0, 0, 0, 0, 0, 0, 0] command_pub.publish(command)
def publish_joint_states(self):
js = JointState()
js.name = []
js.header.stamp = rospy.get_rostime()
js.position = []
js.effort = []
for i in range(len(self.sensors)):
js.name.append(self.names_webots_to_bitbots[self.motor_names[i]])
value = self.sensors[i].getValue()
js.position.append(value)
js.effort.append(self.motors[i].getTorqueFeedback())
self.pub_js.publish(js)
def cb_robot_joint_state(self, state_msg):
if self._t_last_update is None:
self._t_last_update = Time.now()
return
now = Time.now()
dt = (now - self._t_last_update).to_sec()
self._t_last_update = now
self.state[self._poi_pos] = cos(now.to_sec())
if self.robot_js_aliases is None:
return
for name, p in zip(state_msg.name, state_msg.position):
if name in self.robot_js_aliases:
self.state[self.robot_js_aliases[name]] = p
cmd = {}
if self._current_target is None:
if self.idle_controller is not None:
print('Idling around...')
cmd = self.idle_controller.get_cmd(self.state, deltaT=dt)
else:
if self.pushing_controller is not None:
if not self._needs_odom or self._has_odom:
try:
now = Time.now()
cmd = self.pushing_controller.get_cmd(self.state,
deltaT=dt)
time_taken = Time.now() - now
print('Command generated. Time taken: {} Rate: {} hz'.
format(time_taken.to_sec(),
1.0 / time_taken.to_sec()))
except Exception as e:
time_taken = Time.now() - now
print(
'Command generation failed. Time taken: {} Rate: {} hz\nError: {}'
.format(time_taken.to_sec(),
1.0 / time_taken.to_sec(), e))
#print(self.pushing_controller.last_matrix_str())
else:
print('Waiting for odom...')
if len(cmd) > 0:
#print('commands:\n {}'.format('\n '.join(['{}: {}'.format(s, v) for s, v in cmd.items()])))
cmd_msg = JointStateMsg()
cmd_msg.header.stamp = Time.now()
cmd_msg.name, cmd_msg.velocity = zip(
*[(self.inverse_js_aliases[erase_type(s)], v)
for s, v in cmd.items()])
cmd_msg.position = [0] * len(cmd_msg.name)
cmd_msg.effort = cmd_msg.position
self.pub_cmd.publish(cmd_msg)
def main_fcn():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
pub2 = rospy.Publisher('read_angles', Int32, queue_size=10)
pub3 = rospy.Publisher('uarm_status', String, queue_size=100)
rospy.init_node('display', anonymous=True)
rate = rospy.Rate(20)
joint_state_send = JointState()
joint_state_send.header = Header()
joint_state_send.name = [
'base_to_base_rot', 'base_rot_to_link_1', 'link_1_to_link_2',
'link_2_to_link_3', 'link_3_to_link_end'
]
joint_state_send.name = joint_state_send.name + [
'link_1_to_add_1', 'link_2_to_add_4', 'link_3_to_add_2',
'base_rot_to_add_3', 'link_2_to_add_5'
]
angle = {}
trigger = 1
while not rospy.is_shutdown():
joint_state_send.header.stamp = rospy.Time.now()
pub2.publish(1)
if trigger == 1:
pub3.publish("detach")
time.sleep(1)
trigger = 0
angle['s1'] = rospy.get_param('servo_1') * math.pi / 180
angle['s2'] = rospy.get_param('servo_2') * math.pi / 180
angle['s3'] = rospy.get_param('servo_3') * math.pi / 180
angle['s4'] = rospy.get_param('servo_4') * math.pi / 180
joint_state_send.position = [
angle['s1'], angle['s2'], -angle['s2'] - angle['s3'], angle['s3'],
angle['s4']
]
joint_state_send.position = joint_state_send.position + [
-angle['s2'] - angle['s3'], angle['s3'], PI / 2 - angle['s3']
]
joint_state_send.position = joint_state_send.position + [
angle['s2'] - PI, angle['s2'] + angle['s3'] - PI / 2
]
joint_state_send.velocity = [0]
joint_state_send.effort = []
pub.publish(joint_state_send)
rate.sleep()
def take_action(self, request):
print('taking action ', request.angle.data)
joint_action = JointState()
joint_action.header = Header()
joint_action.header.stamp = rospy.Time.now()
joint_action.name = ['joint1']
joint_action.position = [request.angle.data]
joint_action.velocity = [0.01]
joint_action.effort = []
self.action_pub.publish(joint_action)
time.sleep(2)
return TakeActionResponse(self.get_current_observation().img)
def odomcb(self, data):
self.X = self.odom_to_state(data)
# if self.running_flag:
joint_states= JointState()
joint_states.header = Header()
joint_states.header.stamp = rospy.Time.now()
joint_states.name = ['virtual_x', 'virtual_y', 'virtual_theta']
joint_states.position = [self.X[0], self.X[1], self.X[2]]
joint_states.velocity = []
joint_states.effort = []
self.pub.publish(joint_states)
return
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
theta1, theta2, theta3 = calc_joint_angles(data.x, data.y, data.z)
# pub.publish(data=[theta1, theta2, theta3])
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['Joint1', 'Joint2', 'Joint3']
hello_str.position = [theta1/rad2deggeared, -theta2/rad2deggeared , -(theta3 - theta2)/rad2deggeared]
hello_str.velocity = []
hello_str.effort = []
pub2.publish(hello_str)
def run(self):
if (self.initialized):
status = self.gripper.getStatus()
js = JointState()
js.header.stamp = rospy.Time().now()
js.name = ["gripperjoint_1", "gripperjoint_2", "gripperjoint_3"]
js.position = [
status.gPOA / 255.0, status.gPOB / 255.0, status.gPOC / 255.0
]
js.velocity = [0.0, 0.0, 0.0]
js.effort = [status.gCUA * 10, status.gCUB * 10, status.gCUC * 10]
self.js_pub.publish(js)
def send_joint_state_msg(self, angle):
joint_msg = JointState()
joint_msg.header.frame_id = ""
joint_msg.header.stamp = rospy.Time.now()
joint_msg.name = [
'joint_front', 'joint_wheel_back', 'joint_wheel_left',
'joint_wheel_right'
]
joint_msg.position = [angle, 0.0, 0.0, 0.0]
joint_msg.velocity = [0.0, 0.0, 0.0, 0.0]
joint_msg.effort = [0.0, 0.0, 0.0, 0.0]
self.pub.publish(joint_msg)
def set_position(self,pose,cn):
joint_info=JointState()
joint_info.header.stamp = rospy.Time.now()
joint_info.header.frame_id = "world"
joint_info.header.seq=cn
joint_info.name=self.arm_joints
print "name",len(self.arm_joints)
joint_info.position=pose
print "psoe",len(pose)
joint_info.velocity=[0.1 for i in self.arm_joints]
joint_info.effort=[]
self.joint_states_pub.publish(joint_info)
def get_joint_state_msg(self):
js = JointState()
js.name = []
js.header.stamp = rospy.Time.from_seconds(self.time)
js.position = []
js.effort = []
for i in range(len(self.sensors)):
js.name.append(self.external_motor_names[i])
value = self.sensors[i].getValue()
js.position.append(value)
js.effort.append(self.motors[i].getTorqueFeedback())
return js
def publish_joint_states(self, goal, part):
import math
rate = rospy.Rate(10)
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [part]
joint_state.position = [math.radians(int(goal))]
joint_state.velocity = []
joint_state.effort = []
self.pub.publish(joint_state)
rate.sleep()
def on_joint_state_msg(data):
m = JointState()
m.header.stamp = data.header.stamp
m.name = [
'front_left_wheel', 'front_right_wheel', 'rear_left_wheel',
'rear_right_wheel'
]
m.position = [0, 0, 0, 0]
m.velocity = [0, 0, 0, 0]
m.effort = [0, 0, 0, 0]
pose_pub.publish(m)
def grab(self): #custom grab
next_position = JointState()
name = ['joint_' + str(i) + '.0' for i in range(16)]
velocity = [-3.493018918753451, -40.103029702888406, 120.73586545387582, -36.63389744821353, 252.3295750549728, -115.97092579748497, -7.3510231788609115, 56.5863491018089, -111.2786242991041, -3.362467567000076, -22.549172696996294, 53.08861330082114, 92.40275207505158, -145.99928833604542, -53.02169036962198, 5.906627932489557]
#position = [-0.03169593538969933, 1.4220984148287619, 0.7101407747942525, 1.1176176912241522, 0.13346401524068366, 1.5280307021719746, 0.49009169687580434, 1.2479782045877417, 0.25531029257282734, 1.634564959105132, 0.3227728848421474, 1.3604674336824014, 1.5428447454559708, 0.15895744413651658, 0.4481706633437269, -0.15737117478512253]
pos1 = [-0.04, 1.7, 0, 0,
0.17, 1.68, 0, 0,
0.36, 1.71, 0, 0,
1.5, 0.50, 0, -1]
pos2 = [-0.04, 1.7, 1.4, 0,
0.17, 1.68, 1.37, 0,
0.36, 1.71, 1.37, 0,
1.5, 0.50, 0.5, -1]
pos3 = [-0.04, 1.7, 1.4, 1.7,
0.17, 1.68, 1.37, 1.67,
0.36, 1.71, 1.37, 1.68,
1.5, 0.50, 1, -1]
pos4 = [-0.04, 1.7, 1.4, 1.7,
0.17, 1.68, 1.37, 1.67,
0.36, 1.71, 1.37, 1.68,
1.5, 1.5, 1, 1]
effort = np.array([0.5, 0.75, 0.5, 0.75, 0.5, 0.75, 0.5, 0.75, 0.5, 0.75, 0.5, 0.75, 0.75, 0.75, 0.5, 0.5])
next_position.name.extend(name)
# next_position.velocity.extend(velocity)
next_position.position = pos1
next_position.effort = effort
self.joint_cmd(next_position)
time.sleep(0.5)
next_position.position = pos2
next_position.effort = effort
self.joint_cmd(next_position)
time.sleep(0.5)
next_position.position = pos3
next_position.effort = effort
self.joint_cmd(next_position)
time.sleep(0.5)
next_position.position = pos4
next_position.effort = effort
self.joint_cmd(next_position)
def comm(self, frameLists):
jointPub = rospy.Publisher("joint_states", JointState, queue_size=10)
markerPub = rospy.Publisher("robotMarker", Marker, queue_size=100)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(10)
js = JointState()
js.header = Header()
mk = Marker()
idx = 0
points = []
while not rospy.is_shutdown() and (idx < len(frameLists)):
js.header.stamp = rospy.Time.now()
js.name = [
'floor_to_link_0', 'link_0_to_link_1', 'link_1_to_link_2',
'link_2_to_link_3', 'link_3_to_gripper_pole',
'left_gripper_joint', 'right_gripper_joint'
]
position_list = list(reversed(frameLists[idx]))
position_list.append(position_list[len(position_list) - 1])
position_list[1] -= np.pi / 2
js.position = position_list
js.velocity = []
js.effort = []
mk.header.frame_id = "floor"
mk.header.stamp = rospy.Time.now()
mk.ns = "my_namespace"
mk.id = 0
mk.type = Marker.LINE_STRIP
mk.action = Marker.ADD
thetas = list(reversed(frameLists[idx][2:]))
thetas.insert(4, 0)
endPoint = self.arm.forward_kinematics(thetas)[:3, 3]
p = Point()
p.x = endPoint[1] / -100.0
p.y = endPoint[0] / 100.0
p.z = endPoint[2] / 100.0 + 0.09
points.append(p)
mk.points = points
mk.pose.orientation.w = 1.0
mk.scale.x = 0.01
mk.color.a = 1.0 # Don't forget to set the alpha!
mk.color.g = 1.0
markerPub.publish(mk)
jointPub.publish(js)
idx += 1
if idx == len(frameLists):
points = []
idx = 0
rate.sleep()
def run(self):
self._publish_rate = rospy.Rate(rospy.get_param('~publish_rate', 20.0))
# The name of the joint we are generating states for (default
# laser_tilt_joint)
self._joint_name = rospy.get_param('~joint_name', 'tilt_laser_joint')
self._phase_offset = rospy.get_param('~phase_offset', 0.00)
self._joint_states_publisher = rospy.Publisher('joint_states',
JointState)
self._signal_subscriber = rospy.Subscriber('~signal', LaserTiltSignal,
self._on_laser_tilt_signal)
self._profile_subscriber = rospy.Subscriber('~profile',
LaserTiltProfile,
self._on_laser_profile)
while not rospy.is_shutdown():
self._publish_rate.sleep()
# If we didn't receive a signal or the current configuration
# yet, do nothing.
with self._lock:
if (self._profile is None or self._signal is None
or self._increasing_velocity is None
or self._decreasing_velocity is None):
continue
now = rospy.Time.now()
delta_t = (now - self._signal.header.stamp).to_sec()
if delta_t >= self._profile.increasing_duration + self._profile.decreasing_duration:
rospy.logwarn('No signal received for a complete period.')
self._signal = None
continue
extrapolated_joint_state = None
if self._signal.signal == LaserTiltSignal.ANGLE_INCREASING:
extrapolated_joint_state = self._extrapolate_increasing_angle(
delta_t)
else:
extrapolated_joint_state = self._extrapolate_decreasing_angle(
delta_t)
if (extrapolated_joint_state.position > self._profile.max_angle
or extrapolated_joint_state.position <
self._profile.min_angle):
raise ExtrapolatedPositionInvalid(
'Position %r outside angle limits %r and %r.' %
(extrapolated_joint_state.position,
self._profile.min_angle, self._profile.min_angle))
joint_state = JointState()
joint_state.header.stamp = now + rospy.Duration(self._phase_offset)
joint_state.name = [self._joint_name]
joint_state.position = [extrapolated_joint_state.position]
joint_state.velocity = [extrapolated_joint_state.velocity]
joint_state.effort = [0.0]
self._joint_states_publisher.publish(joint_state)
def headMoveType2(self,yaw,pitch):
pan_pos=float(yaw)
tilt_pos=float(pitch)
pan_ratio = pan_pos/(self.image_size[0]/2.0)
tilt_ratio = tilt_pos/(self.image_size[1]/2.0)
pan_ratio = min(1,pan_ratio)
pan_ratio = max(-1,pan_ratio)
tilt_ratio = min(1,tilt_ratio)
tilt_ratio = max(-1,tilt_ratio)
print "Pan_pos:",pan_ratio,"| Tilt_ratio:",tilt_ratio
msg = JointState()
msg.name = list()
msg.position = list()
msg.velocity = list()
msg.effort = list()
msg.name.append('head_pan_joint')
if pan_ratio>0:
msg.position.append(2)
else:
msg.position.append(-2)
if abs(pan_ratio)<0.2:
pan_ratio = 0.0
elif pan_ratio>0.2:
pan_ratio-=0.2
elif pan_ratio < -0.2:
pan_ratio+=0.2
msg.velocity.append(abs(pan_ratio)*self.head_velocity_factor)
msg.name.append('head_tilt_joint')
if tilt_ratio>0:
msg.position.append(2)
else:
msg.position.append(-2)
if abs(tilt_ratio)<0.2:
tilt_ratio = 0.0
elif tilt_ratio>0.2:
tilt_ratio-=0.2
elif tilt_ratio < -0.2:
tilt_ratio+=0.2
msg.velocity.append(abs(tilt_ratio)*self.head_velocity_factor)
msg.header.stamp = rospy.Time.now()
self.cmd_joints_pub.publish(msg)
def motion():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher')
rate = rospy.Rate(150)
robot = JointState()
robot.position = [-0.75, 1.5, 1.5, -1.5, -1.5]
x = -0.01
y = 0.01
h = 0.005
a = 0
while not rospy.is_shutdown():
robot.header = Header()
robot.header.stamp = rospy.Time.now()
robot.name = [
'base_to_head', 'base_to_right_arm', 'right_arm_to_right_elbow',
'base_to_left_arm', 'left_arm_to_left_elbow'
]
# head
robot.position[0] += h
# right_arm
robot.position[1] += x
robot.position[2] += x
# left_arm
robot.position[3] += y
robot.position[4] += y
robot.velocity = []
robot.effort = []
rospy.loginfo(robot)
pub.publish(robot)
rate.sleep()
# robot joints speed
if robot.position[1] == -1.5000000000000022:
x = 0.01
y = -0.01
h = -0.005
elif robot.position[1] == 0.9999999999999997:
x = 0.01
y = -0.01
h = -0.005
a += 1
elif robot.position[3] == -1.5:
x = -0.01
y = 0.01
h = 0.005
elif robot.position[3] == 1.5:
x = -0.01
y = 0.01
h = 0.005
elif a == 4:
break
def headMoveType2(self, yaw, pitch):
pan_pos = float(yaw)
tilt_pos = float(pitch)
pan_ratio = pan_pos / (self.image_size[0] / 2.0)
tilt_ratio = tilt_pos / (self.image_size[1] / 2.0)
pan_ratio = min(1, pan_ratio)
pan_ratio = max(-1, pan_ratio)
tilt_ratio = min(1, tilt_ratio)
tilt_ratio = max(-1, tilt_ratio)
print "Pan_pos:", pan_ratio, "| Tilt_ratio:", tilt_ratio
msg = JointState()
msg.name = list()
msg.position = list()
msg.velocity = list()
msg.effort = list()
msg.name.append('head_pan_joint')
if pan_ratio > 0:
msg.position.append(2)
else:
msg.position.append(-2)
if abs(pan_ratio) < 0.2:
pan_ratio = 0.0
elif pan_ratio > 0.2:
pan_ratio -= 0.2
elif pan_ratio < -0.2:
pan_ratio += 0.2
msg.velocity.append(abs(pan_ratio) * self.head_velocity_factor)
msg.name.append('head_tilt_joint')
if tilt_ratio > 0:
msg.position.append(2)
else:
msg.position.append(-2)
if abs(tilt_ratio) < 0.2:
tilt_ratio = 0.0
elif tilt_ratio > 0.2:
tilt_ratio -= 0.2
elif tilt_ratio < -0.2:
tilt_ratio += 0.2
msg.velocity.append(abs(tilt_ratio) * self.head_velocity_factor)
msg.header.stamp = rospy.Time.now()
self.cmd_joints_pub.publish(msg)
def __on_packet(self, buf):
state = RobotState.unpack(buf)
self.last_state = state
#import deserialize; deserialize.pstate(self.last_state)
#log("Packet. Mode=%s" % state.robot_mode_data.robot_mode)
if not state.robot_mode_data.real_robot_enabled:
rospy.logfatal(
"Real robot is no longer enabled. Driver is fuxored")
time.sleep(2)
sys.exit(1)
# If the urscript program is not executing, then the driver
# needs to publish joint states using information from the
# robot state packet.
if self.robot_state != self.EXECUTING:
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "From binary state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, jd in enumerate(state.joint_data):
msg.position[i] = jd.q_actual + joint_offsets.get(
joint_names[i], 0.0)
msg.velocity = [jd.qd_actual for jd in state.joint_data]
msg.effort = [0] * 6
pub_joint_states.publish(msg)
self.last_joint_states = msg
# Updates the state machine that determines whether we can program the robot.
can_execute = (state.robot_mode_data.robot_mode
in [RobotMode.READY, RobotMode.RUNNING])
if self.robot_state == self.CONNECTED:
if can_execute:
self.__trigger_ready_to_program()
self.robot_state = self.READY_TO_PROGRAM
elif self.robot_state == self.READY_TO_PROGRAM:
if not can_execute:
self.robot_state = self.CONNECTED
elif self.robot_state == self.EXECUTING:
if not can_execute:
self.__trigger_halted()
self.robot_state = self.CONNECTED
# Report on any unknown packet types that were received
if len(state.unknown_ptypes) > 0:
state.unknown_ptypes.sort()
s_unknown_ptypes = [str(ptype) for ptype in state.unknown_ptypes]
self.throttle_warn_unknown(
1.0, "Ignoring unknown pkt type(s): %s. "
"Please report." % ", ".join(s_unknown_ptypes))
def talker(pos):
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_publisher1')
rate = rospy.Rate(10) # 10hz
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.name = ['joint1']
hello_str.position = [pos]
hello_str.velocity = []
hello_str.effort = []
pub.publish(hello_str)
rate.sleep()
def set_position(self, pose, cn):
joint_info = JointState()
joint_info.header.stamp = rospy.Time.now()
joint_info.header.frame_id = "base"
joint_info.header.seq = cn
joint_info.name = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
joint_info.position = pose
joint_info.velocity = []
joint_info.effort = []
self.joint_states_pub.publish(joint_info)
def _joint_states_callback(self, ur5_joints, gripper_joint):
msg = JointState()
msg.header = ur5_joints.header
msg.name = list(ur5_joints.name)
msg.name.append('gripper_finger_joint')
msg.position = list(ur5_joints.position)
msg.position.append(gripper_joint.gPO / 255.0)
msg.velocity = list(ur5_joints.velocity)
msg.velocity.append(0.0)
msg.effort = list(ur5_joints.effort)
msg.effort.append(gripper_joint.gCU)
# print msg
self._joint_states_pub.publish(msg)
def talker():
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rospy.init_node('joint_state_bhau' , anonymous=False)
rate = rospy.Rate(10) # 10hz
default = JointState()
default.header = Header()
default.header.stamp = rospy.Time.now()
default.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5']
default.position = [0.0, 0.0, 0.0, 0.0, 0.0]
default.velocity = []
default.effort = []
pub.publish(default)
while not rospy.is_shutdown():
hello_str = JointState()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
theta_base = float(input("{:22s}".format("Enter theta_base: ")))
theta_shoulder = float(input("{:22s}".format("Enter theta_shoulder: ")))
theta_elbow = float(input("{:22s}".format("Enter theta_elbow: ")))
theta_claw1 = float(input("{:22s}".format("Enter theta_claw 1: ")))
theta_claw2 = float(input("{:22s}".format("Enter theta_claw 2: ")))
if 0 <= theta_base <= 180.0 and 0 <= theta_shoulder <= 180.0 and 0 <= theta_elbow <= 180.0 and 0.0 <= theta_claw1 <= 180.0 and 0.0 <= theta_claw2 <= 180.0 :
theta_base = (theta_base)*math.pi/180
theta_shoulder = (theta_shoulder)*math.pi/180
theta_elbow = (theta_elbow)*math.pi/180
theta_claw1= (theta_claw1)*math.pi/180
theta_claw2= (theta_claw2)*math.pi/180
hello_str.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5']
hello_str.position = [theta_base , theta_elbow , theta_shoulder, theta_claw1, theta_claw2]
hello_str.velocity = []
hello_str.effort = []
pub.publish(hello_str)
else:
print ("Enter angles in range 0 to 180")
rate.sleep()
def publish_joint_states(self):
msg = JointState()
msg.name = []
msg.header.stamp = self.ros_time
msg.position = []
msg.effort = []
for i in range(len(self.sensors)):
msg.name.append(self.names_webots_to_bitbots[self.motor_names[i]])
value = self.sensors[i].getValue()
msg.position.append(value)
msg.effort.append(self.motors[i].getTorqueFeedback())
self.pub_js.publish(msg)
def _publish_joint_state(self):
# This gets gets called at 30Hz
# do the update
self._update()
# construct the joint state message.
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ['base_to_body', 'body_to_arm1', 'arm1_to_arm2', 'arm2_to_wrist','wrist_to_endeffector']
msg.position = self.states
# we are not using these
msg.velocity = [0.00,0.00,0.00,0.00,0.00]
msg.effort = [0.00,0.00,0.00,0.00,0.00]
# republish the message.
self.joint_state_pub.publish(msg)
