I am using RTDE protocol for sending data to ursim
I am able to send data and it is getting reflected in URsim, but it is not moving.
I did same workflow with moveJ and it worked as expected.
Here is my script for your referal:
Matlab:
function speedJV2(RTDE,jointsSpeed, acceleration, time)
% Write joints speed
fwrite(RTDE,76,‘uint16’) %Message size
fwrite(RTDE,85,‘uint8’) %RTDE package type
fwrite(RTDE,1,‘uint8’) %Recipe id
fwrite(RTDE,0,‘int32’) %speedJ 1st command mapping
fwrite(RTDE,1,‘int32’) %speedJ 2nd command mapping
fwrite(RTDE,jointsSpeed,‘double’) %Joint speed
fwrite(RTDE,acceleration,‘double’) % Acceleration
if nargin == 4
fwrite(RTDE,time,‘double’) % Time
else
fwrite(RTDE,0.002,‘double’) % Time
end
UrScript
This is a demo of RTDE server script.
global jSpeed = [0,0,0,0,0,0]
global pos = [0,0,0,0,0,0]
global vel = 0
global dT = 0
global accel1 = 0
global accel2 = 0
def read_command1():
return read_input_integer_register(1)
end
def read_command2():
return read_input_integer_register(2)
end
thread speedThread():
write_output_integer_register(1, 0) # 0 indicates operation started
while True:
textmsg(“move_thread started”)
textmsg(get_actual_joint_positions())
speedj(jSpeed, a = accel2,t = dT) # URScript movej command
break
end
enter_critical
write_output_integer_register(1, -1) # indicates operation finished
textmsg(“speed_thread finished”)
exit_critical
end
thread moveThread():
write_output_integer_register(1, 0) # 0 indicates operation started
while True:
textmsg(“move_thread started”)
textmsg(get_actual_joint_positions())
movej(pos, a = accel1, v = vel) # URScript movej command
break
end
enter_critical
write_output_integer_register(1, -1) # indicates operation finished
textmsg(“move_thread finished”)
exit_critical
end
def programCommand2():
textmsg(“program command started”)
if ((cmd1 == 0) and (cmd2 ==1)):
# reading joints value
pos_temp = [0,0,0,0,0,0]
pos_temp[0] = read_input_float_register(0)
pos_temp[1] = read_input_float_register(1)
pos_temp[2] = read_input_float_register(2)
pos_temp[3] = read_input_float_register(3)
pos_temp[4] = read_input_float_register(4)
pos_temp[5] = read_input_float_register(5)
accel_temp = read_input_float_register(6) # reading acceleration value
dT_temp = read_input_float_register(7) # reading time value
enter_critical
jSpeed = pos_temp
dT = dT_temp
accel2 = accel_temp
exit_critical
textmsg(jSpeed)
speed_thrd = run speedThread()
join speed_thrd
end
return True
end
def programCommand1():
textmsg(“program command started”)
if ((cmd1 == 1) and (cmd2 ==0)):
# reading joints value
pos_temp = [0,0,0,0,0,0]
pos_temp[0] = read_input_float_register(0)
pos_temp[1] = read_input_float_register(1)
pos_temp[2] = read_input_float_register(2)
pos_temp[3] = read_input_float_register(3)
pos_temp[4] = read_input_float_register(4)
pos_temp[5] = read_input_float_register(5)
vel_temp = read_input_float_register(6) # reading acceleration value
accel_temp = read_input_float_register(7) # reading velocity value
enter_critical
pos = pos_temp
vel = vel_temp
accel1 = accel_temp
exit_critical
textmsg(pos)
move_thrd = run moveThread()
join move_thrd
end
return True
end
keep_executing = True
executing_cmd1 = False
executing_cmd2 = False
while keep_executing:
cmd1 = read_command1()
cmd2 = read_command2()
if ((cmd1 == 0) and (cmd2 ==0)):
executing_cmd1 = False
executing_cmd2 = False
elif ((cmd1 == 1) and (cmd2 ==0)):
if not executing_cmd1:
keep_executing = programCommand1()
end
executing_cmd1 = True
elif ((cmd1 == 0) and (cmd2 ==1)):
if not executing_cmd2:
keep_executing = programCommand2()
end
executing_cmd2 = True
end
sync()
end