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:
