def create_jog_cartesian_command_message(self, sign):
msg_mc = MovementCommand()
msg_mc.move_command = 74
msg_mc.move_type = 76
msg_mc.ovr = 100
# last joint/gripper has a diffrerent message
if self._current_joint == 6:
msg_mc.target.data_type = 88
msg_mc.target.joints_mask = 127
msg_mc.target.joints_data = [0.0] * 10
msg_mc.target.joints_data[self._current_joint] = sign * self._edo_jog_speed
else:
msg_mc.target.data_type = 80
if self._current_joint == 0:
msg_mc.target.cartesian_data.x = sign * self._edo_jog_speed
elif self._current_joint == 1:
msg_mc.target.cartesian_data.a = sign * self._edo_jog_speed
elif self._current_joint == 2:
msg_mc.target.cartesian_data.y = sign * self._edo_jog_speed
elif self._current_joint == 3:
msg_mc.target.cartesian_data.e = sign * self._edo_jog_speed
elif self._current_joint == 4:
msg_mc.target.cartesian_data.z = sign * self._edo_jog_speed
elif self._current_joint == 5:
msg_mc.target.cartesian_data.r = sign * self._edo_jog_speed
msg_mc.target.joints_mask = 127
return msg_mc
def create_jog_joint_command_message(self, sign):
msg_mc = MovementCommand()
msg_mc.move_command = 74
msg_mc.move_type = 74
msg_mc.ovr = 100
msg_mc.target.data_type = 74
msg_mc.target.joints_mask = 127
msg_mc.target.joints_data = [0.0] * 10
msg_mc.target.joints_data[self._current_joint] = sign * self._edo_jog_speed
return msg_mc
def __init__(self, current_state=-1, opcode=-1, enable_algorithm_node=False, current_command=Button.NONE):
self.edo_current_state = current_state
self._edo_opcode_previous = opcode
self._edo_current_state_previous = current_state
self.edo_opcode = opcode
self._edo_jog_speed = 0.5
self.gripper_position = 60
self.send_first_step_bool = False # select 6-axis configuration
self.send_second_step_bool = False # disconnect the brakes
self.send_third_step_bool = False # calibration process will start
self.current_joint = 0
self.current_joint_states = None
self._sent_next_movement_command_bool = False
self.reselect_joints_bool = False
self.disengage_brakes_timer = None
# self.disengage_brakes_bool = False
self.disengage_brakes_bool = True
self.read_input_bool = False
self.msg_jca = JointControlArray()
self.msg_mc = MovementCommand()
# Joint Command topics
self.jog_command_pub = None
self.joint_control_pub = None
rospy.Subscriber("/machine_state", MachineState, self.callback)
rospy.Subscriber("usb_jnt_state", JointStateArray, self.js_callback)
rospy.Subscriber("open_gripper", Bool, self.callback_gripper)
self._joint_init_command_pub = rospy.Publisher('/bridge_init', JointInit, queue_size=10, latch=True)
self._joint_reset_command_pub = rospy.Publisher('/bridge_jnt_reset', JointReset, queue_size=10, latch=True)
self.jog_command_pub = rospy.Publisher('/bridge_jog', MovementCommand, queue_size=10, latch=True)
self.joint_control_pub = rospy.Publisher('/algo_jnt_ctrl', JointControlArray, queue_size=1)
self.movement_command_pub = rospy.Publisher('/bridge_move', MovementCommand, queue_size=10, latch=True)
self._joint_calibration_command_pub = rospy.Publisher('/bridge_jnt_calib', JointCalibration, queue_size=10,
latch=True)
self._emergency_stop_pub = rospy.Publisher('/emergency_stop', Bool, queue_size=10, latch=True)
self._emergency_stop_pub.publish(Bool(False))
rospy.Subscriber('/machine_movement_ack', MovementFeedback, self.move_ack_callback)
# collision disabled to prevent robot from falling during movement
# TODO alternative solution possible ?
self.disable_collision()
self.switch_algo_service(enable_algorithm_node)
self.current_command = current_command
def move_joint(self, joint_values, joint_speed):
msg_mc = MovementCommand()
msg_mc.move_command = 77
msg_mc.move_type = 74
msg_mc.ovr = joint_speed
msg_mc.delay = 255
msg_mc.target.data_type = 74
msg_mc.target.joints_mask = 127
msg_mc.target.joints_data = [0.0] * 7
# joint 7 are in degress
msg_mc.target.joints_data[0] = joint_values[0]
msg_mc.target.joints_data[1] = joint_values[1]
msg_mc.target.joints_data[2] = joint_values[2]
msg_mc.target.joints_data[3] = joint_values[3]
msg_mc.target.joints_data[4] = joint_values[4]
msg_mc.target.joints_data[5] = joint_values[5]
# joint 7 is in milimeters
msg_mc.target.joints_data[6] = joint_values[6]
self.send_movement_command(msg_mc)
while not self._sent_next_movement_command_bool and not rospy.is_shutdown():
rospy.sleep(0.01)
rospy.loginfo("Move joint should be completed")
def move_home(self):
rospy.loginfo("Sending home position")
# send reset command
msg_mc = MovementCommand()
msg_mc.move_command = 67
self.send_movement_command_init(msg_mc)
msg_mc = MovementCommand()
msg_mc.move_command = 77
msg_mc.move_type = 74
msg_mc.ovr = 50
msg_mc.target.data_type = 74
msg_mc.target.joints_mask = (1 << self.NUMBER_OF_JOINTS) - 1
msg_mc.target.joints_data = [0.0] * 7
self.send_movement_command(msg_mc)
rospy.loginfo("Home position should be soon reached")
def createMove(self, type):
msg = MovementCommand()
msg.move_command = 77
msg.move_type = 74
msg.ovr = 100
msg.delay = 0
if type == "joint":
msg.target.data_type = 74
elif type == "cart":
msg.target.data_type = 88
else:
msg.target.data_type = 74
msg.target.joints_mask = 127
return msg
def execute_move_joint(self):
rospy.loginfo("Setting up a joint movement:")
try:
repeat_number = int(raw_input('Write number of repetions cycles: '))
except ValueError:
rospy.loginfo("Not a number")
rospy.loginfo("Exiting to a main loop.")
return
try:
number_of_points = int(raw_input('Write number of different points: '))
except ValueError:
rospy.loginfo("Not a number")
rospy.loginfo("Exiting to a main loop.")
return
list_of_poses = []
rospy.loginfo("Input: j1 j2 j3 j4 j5 j6 gripper_span")
rospy.loginfo("Joint data in degrees and gripper span in mm")
for i in range(0, number_of_points):
try:
j1, j2, j3, j4, j5, j6, gripper_span = [int(x) for x in raw_input("Input: ").split()]
except ValueError:
rospy.loginfo("Not a valid angles or gripper span...")
rospy.loginfo("Exiting to a main loop.")
return
list_of_poses.append([j1, j2, j3, j4, j5, j6, gripper_span])
for item in list_of_poses:
if len(item) != self.NUMBER_OF_JOINTS:
rospy.logerr("To many or not enough joint values for a robot")
rospy.loginfo("Exiting to a main loop.")
return
try:
joint_speed = int(raw_input('Write joint speed [0,..,100]: '))
except ValueError:
rospy.loginfo("Not a number")
rospy.loginfo("Exiting to a main loop.")
return
joint_speed %= 100
# send reset message
msg_mc = MovementCommand()
msg_mc.move_command = 67
self.send_movement_command_init(msg_mc)
# send all the data
for i in range(0, repeat_number):
for j in range(0, number_of_points):
if not rospy.is_shutdown():
self.move_joint(list_of_poses[j], joint_speed)
rospy.loginfo(list_of_poses[j])
if rospy.is_shutdown():
rospy.loginfo("Terminating an execution loop...")
rospy.loginfo("Exiting to a main loop.")
return
rospy.loginfo("Joint movement was successful.")
rospy.loginfo("Exiting to a main loop.")
return
float(parsed[0]),
float(parsed[1]),
float(parsed[2]),
float(parsed[3]),
float(parsed[4]),
float(parsed[5])
]
bit_flag = 63
#machine_move_pub = rospy.Publisher('machine_move', MovementCommand)
#print mmmsg
ros_cartesian_pose = CartesianPose(0.0,0.0,0.0,0.0,0.0,0.0,'')
ros_point = Point(74, ros_cartesian_pose, bit_flag, joints)
ros_frame = Frame(0.0,0.0,0.0,0.0,0.0,0.0)
mmmsg = MovementCommand(77, 74, 25, 0, 0, 0.0, ros_point, ros_point, ros_frame, ros_frame)
# publish move
machine_move.publish(mmmsg)
imsg = JointInit(3, 127, 10.0)
# publish init to remove collision
machine_init.publish(imsg)
while waitme == 0:
time.sleep(1)
#print("after waitme")
control_switch(1)
try:
os.system("kill $(ps aux | grep 'edo_algorithms' | awk '{print $2}')")
s.send("<GET_ARM_JNT;1>")
data = s.recv(1024)
parsed = parse("<GET_ARM_JNT;1;{};{};{};{};{};{};{};{};{};{}>",
data).fixed
joints = [
float(parsed[0]),
float(parsed[1]),
float(parsed[2]),
float(parsed[3]),
float(parsed[4]),
float(parsed[5])
]
#machine_move_pub = rospy.Publisher('machine_move', MovementCommand)
mmmsg = MovementCommand(0, len(joints), 25, joints, '0')
#print mmmsg
# publish move
machine_move.publish(mmmsg)
while waitme == 0:
time.sleep(1)
#print("after waitme")
control_switch(1)
os.system("kill $(ps aux | grep 'edo_algorithms' | awk '{print $2}')")
#print("after control switch")
while not rospy.is_shutdown() and startc5g == 1:
s.send("<GET_ARM_JNT;1>")