def arm_move_jspace_path(self,
path,
max_jntvel=None,
max_jntacc=None,
start_frame_id=1,
toggle_debug=False):
"""
:param path: [jnt_values0, jnt_values1, ...], results of motion planning
:param max_jntvel:
:param max_jntacc:
:param start_frame_id:
:return:
"""
if not path or path is None:
raise ValueError("The given is incorrect!")
control_frequency = .005
tpply = pwp.PiecewisePolyTOPPRA()
interpolated_path = tpply.interpolate_by_max_spdacc(
path=path,
control_frequency=control_frequency,
max_vels=max_jntvel,
max_accs=max_jntacc,
toggle_debug=toggle_debug)
interpolated_path = interpolated_path[start_frame_id:]
for jnt_values in interpolated_path:
self._arm_x.set_servo_angle_j(jnt_values, is_radian=True)
return
def __init__(self, server_ip="192.168.0.2", server_port=18400):
self._pc_server_socket_addr = (server_ip, server_port)
self._pc_server_socket = socket.socket(socket.AF_INET,
socket.SOCK_STREAM)
self._pc_server_socket.connect(self._pc_server_socket_addr)
self._jnts_scaler = 1e6
self.traj_gen = trajp.PiecewisePolyTOPPRA()
def arm_move_jspace_path(self,
path,
max_jntvel=None,
max_jntacc=None,
start_frame_id=1):
"""
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.PiecewisePolyTOPPRA()
interpolated_path = tpply.interpolate_by_max_spdacc(
path=path,
control_frequency=control_frequency,
max_jntvel=max_jntvel,
max_jntacc=max_jntacc)
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 __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.PiecewisePolyTOPPRA()
import os
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.cobotta.cobotta_ripps as cbtr
import robot_sim.end_effectors.gripper.cobotta_pipette.cobotta_pipette as cbtg
import modeling.collision_model as cm
import visualization.panda.world as wd
import modeling.geometric_model as gm
import utils
import motion.probabilistic.rrt_connect as rrtc
import motion.trajectory.piecewisepoly_toppra as trajp
if __name__ == '__main__':
base = wd.World(cam_pos=[1.7, .7, .7], lookat_pos=[0, 0, 0])
traj_gen = trajp.PiecewisePolyTOPPRA()
max_vels = [np.pi * .6, np.pi * .4, np.pi, np.pi, np.pi, np.pi * 1.5]
gm.gen_frame().attach_to(base)
this_dir, this_filename = os.path.split(__file__)
file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl")
frame_bottom = cm.CollisionModel(file_frame)
frame_bottom.set_rgba([.55, .55, .55, 1])
frame_bottom.attach_to(base)
table_plate = cm.gen_box(extent=[.405, .26, .003])
table_plate.set_pos([0.07 + 0.2025, .055, .0015])
table_plate.set_rgba([.87, .87, .87, 1])
table_plate.attach_to(base)
file_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
dispose_box = cm.CollisionModel(file_dispose_box, expand_radius=.007)
while True:
robot_s = cbt.Cobotta()
start_conf = robot_s.get_jnt_values(component_name="arm")
print("start_radians", start_conf)
tgt_pos = np.array([0, -.2, .15])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 3)
jnt_values = robot_s.ik(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat)
rrtc_planner = rrtc.RRTConnect(robot_s)
path = rrtc_planner.plan(component_name="arm",
start_conf=start_conf,
goal_conf=jnt_values,
ext_dist=.1,
max_time=300)
for pose in path:
robot_s.fk("arm", pose)
robot_meshmodel = robot_s.gen_meshmodel()
robot_meshmodel.attach_to(base)
# tg = trajp.PiecewisePolyScl(method="quintic")
# tg = trajpopt.PiecewisePolyOpt(method="quintic")
# tg = trajpsecopt.PiecewisePolySectionOpt(method="quintic")
tg = trajptop.PiecewisePolyTOPPRA()
interpolated_confs = tg.interpolate_by_max_spdacc(
path,
control_frequency=.008,
max_vels=[math.pi / 2] * 6,
max_accs=[math.pi] * 6,
toggle_debug=True)
# base.run()