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()