def __init__(self, robot_ip='10.2.0.50', pc_ip='10.2.0.91'):
"""
:param robot_ip:
:param pc_ip:
"""
# setup arm
self._arm = urrobot.URRobot(robot_ip)
self._arm.set_tcp((0, 0, 0, 0, 0, 0))
self._arm.set_payload(1.0)
# setup hand
self._hand = r2f.RobotiqETwoFinger(type='hande')
# setup ftsensor
self._ftsensor = rft.RobotiqFT300()
self._ftsensor_socket_addr = (robot_ip, 63351)
self._ftsensor_urscript = self._ftsensor.get_program_to_run()
# setup pc server
self._pc_server_socket_addr = (pc_ip, 0) # 0: the system finds an available port
self._pc_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._pc_server_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self._pc_server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self._pc_server_socket.bind(self._pc_server_socket_addr)
self._pc_server_socket.listen(5)
self._jointscaler = 1e6
self._pb = pb.ProgramBuilder()
script_dir = os.path.dirname(__file__)
self._pb.load_prog(os.path.join(script_dir, "urscripts_cbseries/moderndriver_eseries.script"))
self._pc_server_urscript = self._pb.get_program_to_run()
self._pc_server_urscript = self._pc_server_urscript.replace("parameter_ip", self._pc_server_socket_addr[0])
self._pc_server_urscript = self._pc_server_urscript.replace("parameter_port",
str(self._pc_server_socket.getsockname()[1]))
self._pc_server_urscript = self._pc_server_urscript.replace("parameter_jointscaler",
str(self._jointscaler))
self._ftsensor_thread = None
self._ftsensor_values = []
self.trajt = pwp.PiecewisePoly(method='quintic')
def arm_move_jspace_path(self,
path,
max_jntspeed=math.pi,
method='linear',
start_frame_id=1,
toggle_debug=False):
"""
TODO: make speed even
:param path: [jnt_values0, jnt_values1, ...], results of motion planning
:return:
author: weiwei
date: 20190417
"""
if not path or path is None:
raise ValueError("The given is incorrect!")
control_frequency = .005
tpply = pwp.PiecewisePoly(method=method)
interpolated_path, interpolated_spd, interpolated_acc, interpolated_x = \
tpply.interpolate(path=path, control_frequency=.005, time_intervals=.1)
# tpply.interpolate_by_max_jntspeed(path=path,
# control_frequency=control_frequency,
# max_jntspeed=max_jntspeed)
if toggle_debug:
import matplotlib.pyplot as plt
# plt.plot(interplated_path)
plt.subplot(311)
for i in range(len(path)):
plt.axvline(x=i)
plt.plot(interpolated_path)
plt.subplot(312)
for i in range(len(path)):
plt.axvline(x=i)
plt.plot(interpolated_spd)
plt.subplot(313)
for i in range(len(path)):
plt.axvline(x=i)
plt.plot(interpolated_acc)
plt.show()
import pickle
pickle.dump(
[interpolated_path, interpolated_spd, interpolated_acc],
open("interpolated_traj.pkl", "wb"))
interpolated_path = interpolated_path[start_frame_id:]
path_msg = aa_msg.Path(length=len(interpolated_path),
njnts=len(interpolated_path[0]),
data=np.array(interpolated_path).tobytes())
return_value = self.stub.arm_move_jspace_path(path_msg)
if return_value == aa_msg.Status.ERROR:
print("Something went wrong with the server!! Try again!")
raise Exception()
else:
print("The rbt_s has finished the given motion.")
def move_jspace_path(self, path, time_interval, toggle_debug=False):
"""
:param path: [jnt_values0, jnt_values1, ...], results of motion planning
:return:
author: weiwei
date: 20190417
"""
if not path or path is None:
raise ValueError("The given is incorrect!")
control_frequency = .005
tpply = pwply.PiecewisePoly(method='linear')
interpolated_path, interpolated_spd, interpolated_acc = tpply.interpolate_by_time_interval(
path=path,
control_frequency=control_frequency,
time_interval=time_interval)
if toggle_debug:
import matplotlib.pyplot as plt
# plt.plot(interplated_path)
samples = np.linspace(0,
time_interval,
math.floor(
time_interval / control_frequency),
endpoint=False) / time_interval
nsample = len(samples)
plt.subplot(311)
for i in range(len(path)):
plt.axvline(x=(nsample - 1) * i)
plt.plot(interpolated_path)
plt.subplot(312)
for i in range(len(path)):
plt.axvline(x=(nsample - 1) * i)
plt.plot(interpolated_spd)
plt.subplot(313)
for i in range(len(path)):
plt.axvline(x=(nsample - 1) * i)
plt.plot(interpolated_acc)
plt.show()
import pickle
pickle.dump(
[interpolated_path, interpolated_spd, interpolated_acc],
open("interpolated_traj.pkl", "wb"))
path_msg = xarm_msg.Path(length=len(interpolated_path),
njnts=len(interpolated_path[0]),
data=np.array(interpolated_path).tobytes())
return_value = self.stub.move_jspace_path(path_msg)
if return_value == xarm_msg.Status.ERROR:
print("Something went wrong with the server!! Try again!")
raise Exception()
else:
print("The rbt_s has finished the given motion.")
def __init__(self, host='192.168.0.1', port=5007, timeout=2000):
"""
:param host:
:param port:
:param timeout:
author: weiwei
date: 20210507
"""
self.bcc = bcapclient.BCAPClient(host, port, timeout)
self.bcc.service_start("")
# Connect to RC8 (RC8(VRC)provider)
self.hctrl = self.bcc.controller_connect("", "CaoProv.DENSO.VRC",
("localhost"), (""))
self.clear_error()
# get robot_s object hanlde
self.hrbt = self.bcc.controller_getrobot(self.hctrl, "Arm", "")
# self.bcc.controller_getextension(self.hctrl, "Hand", "")
# take arm
self.hhnd = self.bcc.robot_execute(self.hrbt, "TakeArm", [0, 0])
# motor on
self.bcc.robot_execute(self.hrbt, "Motor", [1, 0])
# set ExtSpeed = [speed, acc, dec]
self.bcc.robot_execute(self.hrbt, "ExtSpeed", [100, 100, 100])
self.traj_gen = trajp.PiecewisePoly(method="cubic")