tip_rack = utils.Rack96(file_tip_rack)
tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
tip_rack.set_pose(pos=np.array([.25, 0.0, .003]),
rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
tip_rack.attach_to(base)
file_tip = os.path.join(this_dir, "objects", "tip.stl")
tip = cm.CollisionModel(file_tip)
tip.set_rgba([200 / 255, 180 / 255, 140 / 255, 1])
for id_x in range(8):
for id_y in range(12):
pos, rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
tip_new = tip.copy()
tip_new.set_pose(pos, rotmat)
tip_new.attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
ee_s = cbtg.CobottaPipette()
pos, rotmat = tip_rack.get_rack_hole_pose(id_x=6, id_y=3)
z_offset = np.array([0, 0, .03])
utils.search_reachable_configuration(rbt_s=rbt_s,
ee_s=ee_s,
component_name="arm",
tgt_pos=pos,
cone_axis=-rotmat[:3, 2],
rotation_interval=np.radians(15),
obstacle_list=[frame_bottom],
toggle_debug=True)
base.run()
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.2, 1.2, .5], lookat_pos=[0, 0, .2])
gm.gen_frame().attach_to(base)
# robot_s
component_name = 'arm'
robot_s = cbt.CobottaRIPPS()
tgt_pos = np.array([0.25, .0, .15])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2).dot(
rm.rotmat_from_axangle([0, 0, 1], 0))
jnt_values = robot_s.ik(component_name=component_name,
tgt_pos=tgt_pos,
tgt_rotmat=tgt_rotmat)
if jnt_values is None:
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
base.run()
robot_s.fk(component_name=component_name, jnt_values=jnt_values)
# pos, rotmat = robot_s.get_gl_tcp(manipulator_name=component_name)
# angle = rm.angle_between_vectors(rotmat[:, 1], np.array([0,1,0]))
# print(angle)
# robot_s.gen_meshmodel().attach_to(base)
# base.run()