def __init__(self, robot_s):
'''
:param robot_s:
'''
self.robot_s = robot_s
self.inik_slvr = inik.IncrementalNIK(self.robot_s)
self.rrtc_planner = rrtc.RRTConnect(self.robot_s)
def __init__(self, robot_s):
"""
:param robot_s:
author: weiwei, hao
date: 20191122, 20210113
"""
self.robot_s = robot_s
self.inik_slvr = inik.IncrementalNIK(self.robot_s)
self.rrtc_planner = rrtc.RRTConnect(self.robot_s)
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
use_real=False,
create_sim_world=True,
lft_robot_ip='10.2.0.50',
rgt_robot_ip='10.2.0.51',
pc_ip='10.2.0.100',
cam_pos=np.array([2, 1, 3]),
lookat_pos=np.array([0, 0, 1.1]),
auto_cam_rotate=False):
self.robot_s = ur3ds.UR3Dual(pos=pos, rotmat=rotmat)
self.rrt_planner = rrtc.RRTConnect(self.robot_s)
self.inik_solver = inik.IncrementalNIK(self.robot_s)
self.pp_planner = ppp.PickPlacePlanner(self.robot_s)
if use_real:
self.robot_x = ur3dx.UR3DualX(lft_robot_ip=lft_robot_ip,
rgt_robot_ip=rgt_robot_ip,
pc_ip=pc_ip)
if create_sim_world:
self.sim_world = wd.World(cam_pos=cam_pos,
lookat_pos=lookat_pos,
auto_cam_rotate=auto_cam_rotate)
object_holder.set_rotmat(object_holder_gl_rotmat)
gm.gen_frame().attach_to(object_holder)
object_holder_copy = object_holder.copy()
object_holder_copy.attach_to(base)
# object holder goal
object_holder_gl_goal_pos = np.array([.25, -.05, .05])
object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos,
object_holder_gl_goal_rotmat)
object_holder_goal_copy = object_holder.copy()
object_holder_goal_copy.set_homomat(obgl_goal_homomat)
object_holder_goal_copy.attach_to(base)
robot_s = cbt.Cobotta()
# robot_s.gen_meshmodel().attach_to(base)
rrtc_s = rrtc.RRTConnect(robot_s)
ppp_s = ppp.PickPlacePlanner(robot_s)
original_grasp_info_list = gpa.load_pickle_file('holder', './',
'cobg_holder_grasps.pickle')
manipulator_name = "arm"
hand_name = "hnd"
start_conf = robot_s.get_jnt_values(manipulator_name)
conf_list, jawwidth_list, objpose_list = \
ppp_s.gen_pick_and_place_motion(hnd_name=hand_name,
objcm=object_holder,
grasp_info_list=original_grasp_info_list,
start_conf=start_conf,
end_conf=start_conf,
goal_homomat_list=[obgl_start_homomat, obgl_goal_homomat],
approach_direction_list=[None, np.array([0, 0, -1])],
robot_instance.fk("rgt_arm", path[0][1])
start_rgt = robot_instance.get_gl_tcp("rgt_arm")
start_lft = robot_instance.fk("lft_arm", path[0][2])
goal_rgt = robot_instance.fk("rgt_arm", path[-1][1])
goal_lft = robot_instance.fk("lft_arm", path[-1][2])
# keyposes.append(start_rgt*np.pi/180)
# robot_instance.fk("rgt_arm", numikrmsmp[4][0][1]*np.pi/180)
# start_config_rgt = robot_instance.get_gl_tcp("rgt_arm")
#
# robot_instance.fk("rgt_arm", numikrmsmp[4][-1][1] * np.pi / 180)
# goal_config_rgt = robot_instance.get_gl_tcp("rgt_arm")
# robot_instance.gen_meshmodel(rgba=(0,1,1,1),dual = "dual").attach_to(base)
rrtc_planner = rrtc.RRTConnect(robot_instance)
path_0 = rrtc_planner.plan(
start_conf=robot_instance.get_jnt_values("rgt_arm"),
goal_conf=numikrmsmp[4][0][1] * np.pi / 180,
obstacle_list=[object, phoxicam],
ext_dist=.1,
max_time=300,
component_name=component_name)
path_1 = rrtc_planner.plan(start_conf=numikrmsmp[4][0][1] * np.pi / 180,
goal_conf=numikrmsmp[4][-1][1] * np.pi / 180,
obstacle_list=[object, phoxicam],
ext_dist=.1,
max_time=300,
component_name=component_name)
path = [path_0, path_1]
old_path = [a[1] * np.pi / 180 for a in numikrmsmp[4]]
base = wd.World(cam_pos=[2, 1, 3], lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
# object
object = cm.CollisionModel("objects/bunnysim.stl")
object.set_pos(np.array([.55, -.3, 1.3]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
# robot_s
component_name = 'lft_arm'
robot_s = ur3d.UR3Dual()
# possible right goal np.array([0, -math.pi/4, 0, math.pi/2, math.pi/2, math.pi / 6])
# possible left goal np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6])
rrtc_planner = rrtc.RRTConnect(robot_s)
path = rrtc_planner.plan(component_name=component_name,
start_conf=robot_s.lft_arm.homeconf,
goal_conf=np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6]),
obstacle_list=[object],
ext_dist=.2,
max_time=300)
print(path)
for pose in path:
print(pose)
robot_s.fk(component_name, pose)
robot_meshmodel = robot_s.gen_meshmodel()
robot_meshmodel.attach_to(base)
robot_s.gen_stickmodel().attach_to(base)
base.run()
microplate24_1.set_pose(pos=np.array([.3, 0.23, -.03]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
microplate24_1.attach_to(base)
microplate24_2 = microplate24_0.copy()
microplate24_2.set_pose(pos=np.array([.15, 0.32, -.03]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
microplate24_2.attach_to(base)
microplate24_3 = microplate24_0.copy()
microplate24_3.set_pose(pos=np.array([.3, 0.32, -.03]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
microplate24_3.attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
init_joint_values = np.radians(np.asarray([0.0, 0.0, 70.0, 0.0, 90.0, 0.0]))
component_name = "arm"
rbt_s.fk(component_name=component_name, jnt_values=init_joint_values)
rbt_s.gen_meshmodel().attach_to(base)
planner = rrtc.RRTConnect(rbt_s)
ee_s = cbtg.CobottaPipette()
id_x = 0
id_y = 0
tip_pos, tip_rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
z_offset = np.array([0, 0, .07])
previous_jnt_values = rbt_s.get_jnt_values(component_name=component_name)
goal_joint_values_attachment = utils.search_reachable_configuration(rbt_s=rbt_s,
ee_s=ee_s,
component_name=component_name,
tgt_pos=tip_pos + z_offset,
cone_axis=-tip_rotmat[:3, 2],
rotation_interval=np.radians(15),
obstacle_list=[frame_bottom])
if goal_joint_values_attachment is not None:
object_fixture_corner_pos,
object_fixture_corner_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1]))
object_fixture_corner.set_homomat(object_fixture_corner_homomat)
object_goal = object.copy()
object_goal_pos = np.array([0.550, -.050, 0.850])
object_goal_rotmat = rm.rotmat_from_axangle(
(0, 0, 1),
np.radians(90)).dot(rm.rotmat_from_axangle((0, 1, 0), np.radians(-90)))
object_goal_homomat = rm.homomat_from_posrot(object_goal_pos,
object_goal_rotmat)
object_goal.set_pos(object_goal_pos)
object_goal.set_rotmat(object_goal_rotmat)
# object_goal.attach_to(base)
# base.run()
rrtc = rrtc.RRTConnect(robot)
ppp = ppp.PickPlacePlanner(robot)
hand_name = "rgt_arm"
start_conf = robot.get_jnt_values(component_name)
# conf_list, jawwidth_list, objpose_list = \
# ppp.gen_pick_and_place_motion(hnd_name=hand_name,
# objcm=object,
# grasp_info_list=grasp_info_list,
# start_conf=start_conf,
# end_conf=start_conf,
# goal_homomat_list=[object_start_homomat, object_fixture_homomat,
# object_start_homomat],
# approach_direction_list=[None, np.array([0, 0, -1]), None],
# approach_distance_list=[.05] * 3,
# depart_direction_list=[np.array([0, 0, 1]), None, None],
if component_name != 'all':
raise ValueError("Only support hnd_name == 'all'!")
return self.jlc.get_jnt_values()
def is_collided(self, obstacle_list=[], otherrobot_list=[]):
for (obpos, size) in obstacle_list:
dist = np.linalg.norm(np.asarray(obpos) - self.get_jntvalues())
if dist <= size / 2.0:
return True # collision
return False # safe
if __name__ == '__main__':
# ====Search Path with RRT====
obstacle_list = [((5, 5), 3), ((3, 6), 3), ((3, 8), 3), ((3, 10), 3),
((7, 5), 3), ((9, 5), 3), ((10, 5), 3), ((10, 0), 3),
((10, -2), 3), ((10, -4), 3), ((0, 12), 3), ((-1, 10), 3),
((-2, 8), 3)] # [x,y,size]
# Set Initial parameters
robot = XYBot()
rrtc_instance = rrtc.RRTConnect(robot)
path = rrtc_instance.plan(component_name='all',
start_conf=np.array([0, 0]),
goal_conf=np.array([5, 10]),
obstacle_list=obstacle_list,
ext_dist=1,
max_time=300,
animation=True)
plt.plot([conf[0] for conf in path], [conf[1] for conf in path], '-k')
plt.show()
