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, .2], rgba=[.7, .5, .3, .7]) object_box_gl_pos = np.array([.5, -.3, .1]) 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) object_box.attach_to(base) object_box.show_cdprimit() robot_s = xsm.XArm7YunjiMobile() robot_s.gen_meshmodel().attach_to(base) rrtc_s = rrtc.RRTConnect(robot_s) inik_s = inik.IncrementalNIK(robot_s) grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_box.pickle') component_name = "arm" gripper_s = xag.XArmGripper() cnter = 0 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) gripper_s.grip_at_with_jcpose(gl_jaw_center_pos, gl_jaw_center_rotmat,
import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm7_shuidi_mobile.xarm7_shuidi_mobile as xav import motion.probabilistic.rrt_connect as rrtc import robot_con.xarm_shuidi.xarm.xarm_client as xac 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 = xav.XArm7YunjiMobile() robot_s.fk(component_name, np.array([0, math.pi * 2 / 3, 0, math.pi, 0, -math.pi / 6, 0])) # robot_x robot_x = xac.XArm7(host="192.168.50.77:18300") init_jnt_angles = robot_x.get_jnt_vlaues() print(init_jnt_angles) rrtc_planner = rrtc.RRTConnect(robot_s) path = rrtc_planner.plan(start_conf=init_jnt_angles, # goal_conf=np.array([math.pi/3, math.pi * 1 / 3, 0, math.pi/2, 0, math.pi / 6, 0]), goal_conf = robot_s.manipulator_dict['arm'].homeconf, obstacle_list=[object], ext_dist= .1, max_time=300, component_name=component_name) robot_x.move_jspace_path(path, time_interval=.1)
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.xarm7_shuidi_mobile.xarm7_shuidi_mobile as rbs import robot_con.xarm_shuidi.xarm_shuidi_client as rbx base = wd.World(cam_pos=[3, 1, 1.5], lookat_pos=[0, 0, 0.7]) rbt_s = rbs.XArm7YunjiMobile() rbt_x = rbx.XArmShuidiClient(host="10.2.0.203:18300") jnt_values = rbt_x.arm_get_jnt_values() jawwidth = rbt_x.arm_get_jawwidth() 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 = .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
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.XArm7YunjiMobile() 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:
import motion.probabilistic.rrt_connect as rrtc import hupackage.hupackage as hu if __name__ == '__main__': base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1]) gm.gen_frame().attach_to(base) object = cm.CollisionModel("./objects/tubebig.stl") object.set_pos(np.array([0.800, -.500, 0.700])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) component_name = 'arm' # robot_instance = ur3d.UR3Dual() # robot_instance = ur3ed.UR3EDual() # robot_instance = sda5.SDA5F() robot_instance = xarm.XArm7YunjiMobile() start_hnd_pos = np.array([0.3, -0.6, 0.800]) start_hnd_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) goal_hnd_pos = np.array([0.3, -0.601, 1.0]) goal_hnd_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) start_jntsangle = robot_instance.ik(component_name, start_hnd_pos, start_hnd_rotmat) goal_jntsangle = robot_instance.ik(component_name, goal_hnd_pos, goal_hnd_rotmat) hu.debugpos(start_hnd_pos, start_hnd_rotmat, base) robot_instance.fk(component_name, start_jntsangle) # gm.gen_frame(pos=start_hnd_pos, rotmat=start_hnd_rotmat).attach_to(base) # gm.gen_frame(pos=goal_hnd_pos, rotmat=goal_hnd_rotmat).attach_to(base) robot_meshmodel = robot_instance.gen_meshmodel(