def estimate():
lda0 = float(window.lineEdit_4.text())
lda1 = float(window.lineEdit_5.text())
lda2 = float(window.lineEdit_6.text())
lda3 = float(window.lineEdit_7.text())
lda4 = float(window.lineEdit_8.text())
lda5 = float(window.lineEdit_9.text())
lda6 = float(window.lineEdit_10.text())
left_daList = [lda0, lda1, lda2, lda3, lda4, lda5, lda6]
rda0 = float(window.lineEdit_11.text())
rda1 = float(window.lineEdit_12.text())
rda2 = float(window.lineEdit_13.text())
rda3 = float(window.lineEdit_14.text())
rda4 = float(window.lineEdit_15.text())
rda5 = float(window.lineEdit_16.text())
rda6 = float(window.lineEdit_17.text())
right_daList = [rda0, rda1, rda2, rda3, rda4, rda5, rda6]
leftEstCpList = angleToCP(left_daList)
rightEstCpList = angleToCP(right_daList)
window.label_75.setText("x_pose: %.2f" % leftEstCpList[0])
window.label_76.setText("y_pose: %.2f" % leftEstCpList[1])
window.label_80.setText("z_pose: %.2f" % leftEstCpList[2])
window.label_82.setText("x_pose: %.2f" % rightEstCpList[0])
window.label_137.setText("y_pose: %.2f" % rightEstCpList[1])
window.label_138.setText("z_pose: %.2f" % rightEstCpList[2])
def leftUpdate(self):
global leftAngleList
global leftVelocityList
global leftCpList
rospy.wait_for_service('gazebo/get_joint_properties')
try:
joints_properties = rospy.ServiceProxy(
'gazebo/get_joint_properties', GetJointProperties)
joint1_properties = joints_properties(robot_prefix +
"/left_arm_joint_1")
ja1 = joint1_properties.position[0]
joint2_properties = joints_properties(robot_prefix +
"/left_arm_joint_2")
ja2 = joint2_properties.position[0]
joint3_properties = joints_properties(robot_prefix +
"/left_arm_joint_3")
ja3 = joint3_properties.position[0]
joint4_properties = joints_properties(robot_prefix +
"/left_arm_joint_4")
ja4 = joint4_properties.position[0]
joint5_properties = joints_properties(robot_prefix +
"/left_arm_joint_5")
ja5 = joint5_properties.position[0]
joint6_properties = joints_properties(robot_prefix +
"/left_arm_joint_6")
ja6 = joint6_properties.position[0]
joint7_properties = joints_properties(robot_prefix +
"/left_arm_joint_7")
ja7 = joint7_properties.position[0]
except rospy.ServiceException as e:
print
"Service call failed: %s" % e
perSecond = 1000 / ui_update_rate
jv1 = (ja1 - leftAngleList[0]) * perSecond
jv2 = (ja2 - leftAngleList[1]) * perSecond
jv3 = (ja3 - leftAngleList[2]) * perSecond
jv4 = (ja4 - leftAngleList[3]) * perSecond
jv5 = (ja5 - leftAngleList[4]) * perSecond
jv6 = (ja6 - leftAngleList[5]) * perSecond
jv7 = (ja7 - leftAngleList[6]) * perSecond
leftAngleList = [ja1, ja2, ja3, ja4, ja5, ja6, ja7]
leftVelocityList = [jv1, jv2, jv3, jv4, jv5, jv6, jv7]
leftCpList = angleToCP(leftAngleList)