def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
name="cobotta",
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
# base plate
self.base_plate = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=np.zeros(0),
name='base_plate_ripps')
self.base_plate.jnts[1]['loc_pos'] = np.array([0, 0, 0.01])
self.base_plate.lnks[0]['mesh_file'] = os.path.join(
this_dir, "meshes", "base_plate_ripps.stl")
self.base_plate.lnks[0]['rgba'] = [.55, .55, .55, 1]
self.base_plate.reinitialize()
# arm
arm_homeconf = np.zeros(6)
arm_homeconf[1] = -math.pi / 6
arm_homeconf[2] = math.pi / 2
arm_homeconf[4] = math.pi / 6
self.arm = cbta.CobottaArm(
pos=self.base_plate.jnts[-1]['gl_posq'],
rotmat=self.base_plate.jnts[-1]['gl_rotmatq'],
homeconf=arm_homeconf,
name='arm',
enable_cc=False)
# gripper
self.gripper_loc_rotmat = rm.rotmat_from_axangle(
[0, 0, 1], np.pi) # 20220607 rotate the pipetting end with 180^o.
self.hnd = cbtp.CobottaPipette(
pos=self.arm.jnts[-1]['gl_posq'],
rotmat=self.arm.jnts[-1]['gl_rotmatq'].dot(
self.gripper_loc_rotmat),
name='hnd_s',
enable_cc=False)
# tool center point
self.arm.jlc.tcp_jnt_id = -1
self.arm.jlc.tcp_loc_pos = self.gripper_loc_rotmat.dot(
self.hnd.jaw_center_pos)
self.arm.jlc.tcp_loc_rotmat = self.gripper_loc_rotmat.dot(
self.hnd.jaw_center_rotmat)
# a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd
self.oih_infos = []
# collision detection
if enable_cc:
self.enable_cc()
# component map
self.manipulator_dict['arm'] = self.arm
self.manipulator_dict['hnd'] = self.arm
self.hnd_dict['hnd'] = self.hnd
self.hnd_dict['arm'] = self.hnd
# 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()
arm_s = cbta.CobottaArm(pos=[0, 0, .01])
id_x = 6
id_y = 3
tip_pos, tip_rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
z_offset = np.array([0, 0, .02])
# base.change_campos_and_lookat_pos(cam_pos=[.3, .0, .12], lookat_pos=tip_pos+z_offset)
spiral_points = rm.gen_3d_equilateral_verts(pos=tip_pos + z_offset,
rotmat=tip_rotmat)
print(spiral_points)
pre_point = None
# for point in spiral_points:
# gm.gen_sphere(point, radius=.00016).attach_to(base)
# if pre_point is not None:
# gm.gen_stick(pre_point, point, thickness=.00012).attach_to(base)
# pre_point = point