from basis import robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xss base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object object = cm.CollisionModel("./objects/bunnysim.stl") object.set_pos(np.array([.85, 0, .37])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) # robot_s component_name = 'arm' robot_s = xss.XArmShuidi() robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base) # base.run() seed_jnt_values = robot_s.get_jnt_values(component_name=component_name) for y in range(-5, 5): tgt_pos = np.array([.3, y * .1, 1]) tgt_rotmat = rm.rotmat_from_euler(0, math.pi / 2, 0) gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base) tic = time.time() jnt_values = robot_s.ik(component_name=component_name, tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, max_niter=500, toggle_debug=False, seed_jnt_values=seed_jnt_values) toc = time.time()
base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3]) gm.gen_frame().attach_to(base) ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7]) ground.set_pos(np.array([0, 0, -.5])) ground.attach_to(base) object_box = cm.gen_box(extent=[.02, .06, .7], rgba=[.7, .5, .3, .7]) object_box_gl_pos = np.array([.5, -.3, .35]) object_box_gl_rotmat = np.eye(3) object_box.set_pos(object_box_gl_pos) object_box.set_rotmat(object_box_gl_rotmat) gm.gen_frame().attach_to(object_box) robot_s = xsm.XArmShuidi() rrtc_s = rrtc.RRTConnect(robot_s) adp_s = adp.ADPlanner(robot_s) grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle') component_name = "arm" gripper_s = xag.XArmGripper() for grasp_info in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info gl_jaw_center_pos = object_box_gl_pos + object_box_gl_rotmat.dot( jaw_center_pos) gl_jaw_center_rotmat = object_box_gl_rotmat.dot(jaw_center_rotmat) conf_path, jw_path = adp_s.gen_approach_motion( component_name, gl_jaw_center_pos,
import math import time import keyboard import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.robots.xarm_shuidi.xarm_shuidi as rbs import robot_con.xarm_shuidi.xarm_shuidi_x as rbx base = wd.World(cam_pos=[3, 1, 1.5], lookat_pos=[0, 0, 0.7]) rbt_s = rbs.XArmShuidi() rbt_x = rbx.XArmShuidiX(ip="10.2.0.203") jnt_values = rbt_x.arm_get_jnt_values() jawwidth = rbt_x.arm_get_jaw_width() rbt_s.fk(jnt_values=jnt_values) rbt_s.jaw_to(jawwidth=jawwidth) rbt_s.gen_meshmodel().attach_to(base) agv_linear_speed = .2 agv_angular_speed = .5 arm_linear_speed = .03 arm_angular_speed = .1 while True: pressed_keys = { 'w': keyboard.is_pressed('w'), 'a': keyboard.is_pressed('a'), 's': keyboard.is_pressed('s'), 'd': keyboard.is_pressed('d'), 'r': keyboard.is_pressed('r'), # x+ global 't': keyboard.is_pressed('t'), # x- global 'f': keyboard.is_pressed('f'), # y+ global 'g': keyboard.is_pressed('g'), # y- global
while True: self.pkx.device_get_capture() color_image_handle = self.pkx.capture_get_color_image() depth_image_handle = self.pkx.capture_get_depth_image() if color_image_handle and depth_image_handle: point_cloud = self.pkx.transform_depth_image_to_point_cloud( depth_image_handle) self.pkx.image_release(depth_image_handle) self.pkx.capture_release() return point_cloud pkx = pk.PyKinectAzure() base = wd.World(cam_pos=[3, 3, 3], lookat_pos=[0, 0, 1]) gm.gen_frame().attach_to(base) xss = xsm.XArmShuidi() xsx = xsc.XArmShuidiClient(host="10.2.0.201:18300") jnt_values = xsx.get_jnt_values() xss.fk(component_name="arm", jnt_values=jnt_values) xss.gen_meshmodel().attach_to(base) # sensor handler sensor_handler = SensorHandler(pkx) # # calibrator # dc = DepthCaliberator(robot_x=xsx, robot_s=xss) # # start pos definition # pos_start, rot_start = (np.array([-0.06960152, 0.01039783, 1.41780278]), # np.array([[-1, 0, 0], # [0, -1, 0], # [0, 0, 1]])) # jnts = xss.ik(component_name="arm", tgt_pos=pos_start, tgt_rotmat=rot_start, max_niter=1000) # xsx.move_jnts(component_name="arm", jnt_values=jnts, time_intervals=1)
import math import basis import numpy as np import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xav if __name__ == '__main__': import copy import motion.probabilistic.rrt_connect as rrtc import visualization.panda.rpc.rviz_client as rv_client # # local code global_frame = gm.gen_frame() # define robot_s and robot_s anime info robot_s = xav.XArmShuidi() robot_meshmodel_parameters = [ None, # tcp_jntid None, # tcp_loc_pos None, # tcp_loc_rotmat False, # toggle_tcpcs False, # toggle_jntscs [0, .7, 0, .3], # rgba 'auto' ] # name # define object and object anime info objfile = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl') obj = cm.CollisionModel(objfile) obj_parameters = [[.3, .2, .1, 1]] # rgba obj_path = [[np.array([.85, 0, .17]), np.eye(3)]] # [pos, rotmat] obj.set_pos(np.array([.85, 0, .17]))
# try: self._agv_x.joy_control(linear_velocity=linear_speed, angular_velocity=angular_speed) time_interval = time_interval - .5 time.sleep(.3) return if __name__ == "__main__": import keyboard import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.robots.xarm_shuidi.xarm_shuidi as rbt base = wd.World(cam_pos=[3, 1, 1.5], lookat_pos=[0, 0, 0.7]) rbt_s = rbt.XArmShuidi() rbt_x = XArmShuidiX(ip="10.2.0.203") jnt_values = rbt_x.arm_get_jnt_values() print(jnt_values) jawwidth = rbt_x.arm_get_jaw_width() rbt_s.fk(jnt_values=jnt_values) rbt_s.jaw_to(jawwidth=jawwidth) rbt_s.gen_meshmodel().attach_to(base) # base.run() # rbt_x.agv_move(agv_linear_speed=-.1, agv_angular_speed=.1, time_intervals=5) agv_linear_speed = .2 agv_angular_speed = .5 arm_linear_speed = .02 arm_angular_speed = .05 while True: pressed_keys = {
object_box.attach_to(base) # object2 object_box2 = object_box.copy() object_box2.set_pos(np.array([1.9, -.5, 0])) object_box2.attach_to(base) # object3 object_box3 = object_box.copy() object_box3.set_pos(np.array([.9, -.5, 0])) object_box3.attach_to(base) # object4 object_box4 = object_box.copy() object_box4.set_pos(np.array([.9, -1, 0])) object_box4.attach_to(base) # robot_s component_name = 'agv' robot_instance = xss.XArmShuidi() rrtc_planner = rrtdwc.RRTDWConnect(robot_instance) path = rrtc_planner.plan( start_conf=np.array([0, 0, 0]), goal_conf=np.array([2, -2, math.radians(190)]), obstacle_list=[object_box, object_box2, object_box3, object_box4], ext_dist=.1, max_time=300, component_name=component_name) # print(path) for pose in path: # print(pose) robot_instance.fk(component_name, pose) robot_meshmodel = robot_instance.gen_meshmodel() robot_meshmodel.attach_to(base) # robot_meshmodel.show_cdprimit()
def genSphere(pos, radius=0.02, rgba=None): if rgba is None: rgba = [1, 0, 0, 1] gm.gen_sphere(pos=pos, radius=radius, rgba=rgba).attach_to(base) if __name__ == '__main__': # nokov client init nokov_client = NokovClient() base = wd.World(cam_pos=[1.5, 0, 3], lookat_pos=[0, 0, .5]) gm.gen_frame().attach_to(base) # robot_s component_name = 'agv' robot_1 = xa.XArmShuidi(enable_cc=True) robot_2 = xa.XArmShuidi(enable_cc=True) def update(rbtmnp, robot_1, robot_2, nokov_client: NokovClient, armname, task): rigidbody_dataframe = nokov_client.get_rigidbody_set_frame() if rigidbody_dataframe is not None: if rbtmnp[0] is not None: rbtmnp[0].detach() if rbtmnp[1] is not None: rbtmnp[1].detach() # robot 1 rigidbodydata = rigidbody_dataframe.rigidbody_set_dict[1] rob_rotmat = rigidbodydata.get_rotmat() offset = np.array([0.16, 0.088, -0.461862]) # gm.gen_frame(pos=rigidbodydata.coord, rotmat=rob_rotmat, length=1).attach_to(base)
def genSphere(pos, radius=0.02, rgba=None): if rgba is None: rgba = [1, 0, 0, 1] gm.gen_sphere(pos=pos, radius=radius, rgba=rgba).attach_to(base) if __name__ == '__main__': base = wd.World(cam_pos=[1.5, 0, 3], lookat_pos=[0, 0, .5]) gm.gen_frame().attach_to(base) # robot_s component_name = 'arm' robot_s = xa.XArmShuidi(enable_cc=True) homeconf = np.radians([0, -60, 0, 15, 0, 15, 0]) # robot_s.gen_meshmodel(rgba=[0,1,0,.4]).attach_to(base) homeconf[1] = -math.pi / 2 robot_s.fk(component_name=component_name, jnt_values=homeconf) robot_s.gen_meshmodel(rgba=[1, 0, 0, .3]).attach_to(base) # null space planning path = [] ratio = .01 for t in range(0, 5000, 1): print("-------- timestep = ", t, " --------") xa_jacob = robot_s.jacobian() # xa_ns = rm.null_space(xa_jacob) # cur_jnt_values = robot_s.get_jnt_values() # cur_jnt_values += np.ravel(xa_ns)*.01
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xss import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object object = cm.CollisionModel("./objects/bunnysim.stl") object.set_pos(np.array([.85, 0, .57])) object.set_rgba([.5, .7, .5, 1]) object.attach_to(base) # robot_s component_name = 'arm' rbt_s = xss.XArmShuidi() rbt_s.fk(component_name, np.array([0, math.pi * 1 / 2, 0, math.pi * .9, 0, -math.pi / 6, 0])) # rbt_s.gen_meshmodel().attach_to(base) # rbt_s.show_cdprimit() # base.run() rrtc_planner = rrtc.RRTConnect(rbt_s) path = rrtc_planner.plan(start_conf=rbt_s.get_jnt_values(), goal_conf=np.array([ math.pi / 3, math.pi * 1 / 3, 0, math.pi / 2, 0, math.pi / 6, 0 ]), obstacle_list=[object], ext_dist=.1, max_time=300, component_name=component_name)
pcd_list = [] ball_center_list = [] para_list = [] ball_list = [] counter = [0] # get background background_image = None while True: pkx.device_get_capture() depth_image_handle = pkx.capture_get_depth_image() if depth_image_handle: background_image = pkx.image_convert_to_numpy(depth_image_handle) pkx.image_release(depth_image_handle) pkx.capture_release() break rbts = xsm.XArmShuidi() pos_start, rot_start = (np.array([-0.06960152, 0.01039783, 1.41780278]), np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])) jnts = rbts.ik(component_name="arm", tgt_pos=pos_start, tgt_rotmat=rot_start, max_niter=1000) rbts.fk(jnt_values=jnts) rbts.gen_meshmodel(toggle_tcpcs=True).attach_to(base) rbtx = xsc.XArmShuidiClient(host="10.2.0.201:18300") rbtx.move_jnts(component_name="arm", jnt_values=jnts) def update(pkx, rbtx, background_image, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: