def initialize():
global timeLastMsgReceived
global watchdogDuration
global watchdogErrorPrinted
watchdogDuration = 3
watchdogErrorPrinted = False
#Link Lengths in meters
global link1Length
link1Length = 0.28575
global link2Length
link2Length = .45085
global link3Length
link3Length = .45085
global link4Length
link4Length = 0.0##50.0
#define each joint as an robolink joint.
joint0 = robolinkJoint()
joint1 = robolinkJoint()
joint2 = robolinkJoint()
joint3 = robolinkJoint()
joint4 = robolinkJoint()
#Set the maximum absolute position in degrees for each joint
joint0.maxAngle = rospy.get_param('/joint_0/Max_Angle', 180)
joint0.minAngle = rospy.get_param('/joint_0/Min_Angle', -180)
joint1.maxAngle = rospy.get_param('/joint_1/Max_Angle', 130)
joint1.minAngle = rospy.get_param('joint_1/Min_Angle', -50)
joint2.maxAngle = rospy.get_param('/joint_2/Max_Angle', 180)
joint2.minAngle = rospy.get_param('/joint_2/Min_Angle', -180)
joint3.maxAngle = rospy.get_param('/joint_3/Max_Angle', 90)
joint3.minAngle = rospy.get_param('/joint_3/Min_Angle', -90)
joint4.maxAngle = rospy.get_param('/joint_4/Max_Angle', 180)
joint4.minAngle = rospy.get_param('/joint_4/Min_Angle', -180)
#Set the home position of each joint. This is the encoder position that results in a 0 degree joint position
joint0.homePos = rospy.get_param('/joint_0/Home_Pos', 0)
joint1.homePos = rospy.get_param('/joint_1/Home_Pos', 0)
joint2.homePos = rospy.get_param('/joint_2/Home_Pos', 0)
joint3.homePos = rospy.get_param('/joint_3/Home_Pos', 0)
joint4.homePos = rospy.get_param('/joint_4/Home_Pos', 0)
#CAN Node IDs for the motors as determined by EPOS Studio.
#Correlation between DIP switch on motherboard and resulting
#Node ID is not known.
joint0.controlMsg.node_id = rospy.get_param('/joint_0/Node_ID', 2)
joint1.controlMsg.node_id = rospy.get_param('/joint_1/Node_ID', 3)
joint2.controlMsg.node_id = rospy.get_param('/joint_2/Node_ID', 16)
joint3.controlMsg.node_id = rospy.get_param('/joint_3/Node_ID', 8)
joint4.controlMsg.node_id = rospy.get_param('/joint_4/Node_ID', 4)
#Put these joints in a list to make referencing them easier
global jointList
jointList = [joint0, joint1, joint2, joint3, joint4]
#joint variables
q0 = 0.0
q1 = 0.0
q2 = 0.0
q3 = 0.0
q4 = 0.0
#Put these joint variables in a list to make referencing them easier
global jointVariablesList
jointVariablesList = [q0, q1, q2, q3, q4]
global jointVelocitiesList
jointVelocitiesList = array('f',[0.0, 0.0, 0.0, 0.0, 0.0])
#DH Parameters
#[transx(a) rotx(alpha) transz(d) rotz(theta)]
robolinkDH = numpy.matrix([[0.0, 90.0, 0.0, 0.0],
[0.0, 90.0, link1Length, 90-jointVariablesList[0]],
[0.0, -90.0, 0.0, jointVariablesList[1]],
[0.0, 90.0, link2Length, jointVariablesList[2]],
[0.0, -90.0, 0.0, jointVariablesList[3]],
[0.0, 90.0, link3Length, jointVariablesList[4]],
[link4Length, 90.0, 0.0, 0.0]])
#Jacobian
'''
global Jacobian
Jacobian = numpy.matrix([[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0]])
'''
#PID constants and storage arrays
global Pconstants
Pconstants = array('d',[20.0, 20.0, 20.0, 20.0, 20.0, 20.0])
global Iconstants
Iconstants = array('d',[0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
global Dconstants
Dconstants = array('d',[0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
global prevErrors
prevErrors = array('d',[0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
global cumulativeErrors
cumulativeErrors = array('d',[0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
global tfListener
tfListener = tf.TransformListener()
global tfBroadcaster
tfBroadcaster = tf.TransformBroadcaster()
setAllVelocities(0, 0, 0, 0, 0)
from sensor_msgs.msg import Joy
from robolink.msg import GroupRobolinkJointInfo
from robolink.msg import RobolinkJointInfo
import sys, select, termios, tty, array
from array import array
from robolink.classes import robolinkJoint
groupDrivePublisher = rospy.Publisher('Group_Motor_Control', GroupEPOSControl)
groupRobolinkJointInfoPublisher = rospy.Publisher('Group_Robolink_Joint_Info', GroupRobolinkJointInfo)
#digitalOutputPublisher = rospy.Publisher('Digital_Output_Control', IOControl)
#define each joint as an robolink joint.
ioController = robolinkJoint()
joint0 = robolinkJoint()
joint1 = robolinkJoint()
joint2 = robolinkJoint()
joint3 = robolinkJoint()
joint4 = robolinkJoint()
#Set the maximum absolute position in degrees for each joint
joint0.maxAngle = rospy.get_param('/joint_0/Max_Angle', 180)
joint0.minAngle = rospy.get_param('/joint_0/Min_Angle', -180)
joint1.maxAngle = rospy.get_param('/joint_1/Max_Angle', 130)
joint1.minAngle = rospy.get_param('joint_1/Min_Angle', -50)
joint2.maxAngle = rospy.get_param('/joint_2/Max_Angle', 180)
joint2.minAngle = rospy.get_param('/joint_2/Min_Angle', -180)