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(