class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
self.pinza = False
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
SCENE_FILE = '../etc/gen3-robolab.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.mode = 0
self.bloqueo = True
#self.robot = Viriato()
#self.robot = YouBot()
self.robot_object = Shape("gen3")
self.cameras = {}
self.camera_arm_name = "camera_arm"
cam = VisionSensor(self.camera_arm_name)
self.cameras[self.camera_arm_name] = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.joystick_newdata = []
self.last_received_data_time = 0
def compute(self):
tc = TimeControl(0.05)
while True:
self.pr.step()
self.read_joystick()
self.read_camera(self.cameras[self.camera_arm_name])
if self.pinza:
self.pr.script_call("close@RG2", 1)
else:
self.pr.script_call("open@RG2", 1)
tc.wait()
###########################################
def read_camera(self, cam):
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(True)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
cam["rgb"] = RoboCompCameraRGBDSimple.TImage(cameraID=cam["id"],
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
cameraID=cam["id"],
width=cam["handle"].get_resolution()[0],
height=cam["handle"].get_resolution()[1],
focalx=cam["focal"],
focaly=cam["focal"],
alivetime=time.time(),
depthFactor=1.0,
depth=depth.tobytes())
###########################################
### JOYSITCK read and move the robot
###########################################
def read_joystick(self):
if self.joystick_newdata:
print(self.joystick_newdata)
self.update_joystick(self.joystick_newdata[0])
self.joystick_newdata = None
self.last_received_data_time = time.time()
else:
elapsed = time.time() - self.last_received_data_time
if elapsed > 2 and elapsed < 3:
pass
def update_joystick(self, datos):
adv = advR = 0.0
rot = rotR = 0.0
side = sideR = 0.0
print(datos.buttons)
for x in datos.buttons:
if x.name == "mode":
self.mode += x.step
if self.mode % 2 == 1:
self.bloqueo = True
else:
self.bloqueo = False
for x in datos.axes:
print(x.name + "" + str(x.value))
if x.name == "X_axis":
adv = x.value if np.abs(x.value) > 1 else 0
advR = x.value if np.abs(x.value) > 1 else 0
if x.name == "Y_axis":
rot = x.value if np.abs(x.value) > 0.01 else 0
rotR = x.value if np.abs(x.value) > 0.01 else 0
if x.name == "Z_axis":
side = x.value if np.abs(x.value) > 1 else 0
sideR = x.value if np.abs(x.value) > 1 else 0
if x.name == "gripper":
if x.value > 1:
self.pr.script_call("open@RG2", 1)
print("abriendo")
if x.value < -1:
print("cerrando")
self.pr.script_call("close@RG2", 1)
dummy = Dummy("target")
parent_frame_object = None
position = dummy.get_position()
orientation = dummy.get_orientation()
if self.bloqueo == False:
print("modo0\n\n")
dummy.set_position([
position[0] + adv / 1000, position[1] + rot / 1000,
position[2] + side / 1000
], parent_frame_object)
else:
print("modo1\n\n")
dummy.set_orientation([
orientation[0] + advR / 1000, orientation[1] + rotR / 1000,
orientation[2] + sideR / 1000
], parent_frame_object)
##################################################################################
# SUBSCRIPTION to sendData method from JoystickAdapter interface
###################################################################################
def JoystickAdapter_sendData(self, data):
self.joystick_newdata = [data, time.time()]
##################################################################################
# Methods for CameraRGBDSimple
# ===============================================================================
#
# getAll
#
def CameraRGBDSimple_getAll(self, camera):
return RoboCompCameraRGBDSimple.TRGBD(self.cameras[camera]["rgb"],
self.cameras[camera]["depth"])
#
# getDepth
#
def CameraRGBDSimple_getDepth(self, camera):
return self.cameras[camera]["depth"]
#
# getImage
#
def CameraRGBDSimple_getImage(self, camera):
return self.cameras[camera]["rgb"]
# ===================================================================
# CoppeliaUtils
# ===================================================================
def CoppeliaUtils_addOrModifyDummy(self, type, name, pose):
if not Dummy.exists(name):
dummy = Dummy.create(0.1)
dummy.set_name(name)
else:
dummy = Dummy("target")
#parent_frame_object = None
parent_frame_object = Shape("gen3")
#print("Coppelia ", name, pose.x/1000, pose.y/1000, pose.z/1000)
#dummy.set_position([pose.x / 1000., pose.y / 1000., pose.z / 1000.], parent_frame_object)
dummy.set_position([pose.x, pose.y, pose.z], parent_frame_object)
print(pose.x, pose.y, pose.z)
print(dummy.get_position())
dummy.set_orientation([pose.rx, pose.ry, pose.rz],
parent_frame_object)
def KinovaArm_getCenterOfTool(self, referencedTo):
ret = RoboCompKinovaArm.TPose()
parent_frame_object = Shape('gen3')
tip = Dummy("tip")
pos = tip.get_position(parent_frame_object)
rot = tip.get_orientation(parent_frame_object)
ret.x = pos[0]
ret.y = pos[1]
ret.z = pos[2]
ret.rx = rot[0]
ret.ry = rot[1]
ret.rz = rot[2]
return ret
def KinovaArm_openGripper(self):
#self.pr.script_call("open@RG2", 1)
print("Opening gripper")
self.pinza = False
def KinovaArm_closeGripper(self):
#self.pr.script_call("close@RG2", 1)
print("Closing gripper")
self.pinza = True
#
# IMPLEMENTATION of setCenterOfTool method from KinovaArm interface
#
def KinovaArm_setCenterOfTool(self, pose, referencedTo):
target = Dummy("target")
parent_frame_object = Shape('gen3')
position = target.get_position(parent_frame_object)
#target.set_position([position[0] + pose.x / 1000, position[1] + pose.y / 1000, position[2] + pose.z / 1000], parent_frame_object)
target.set_position([
position[0] + pose.x / 1000, position[1] + pose.y / 1000,
position[2] + pose.z / 1000
], parent_frame_object)
def KinovaArm_setPosition(self, pose, referencedTo):
target = Dummy("target")
parent_frame_object = Shape('gen3')
position = target.get_position(parent_frame_object)
target.set_position([pose.x, pose.y, pose.z], parent_frame_object)
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
def __del__(self):
print('SpecificWorker destructor')
self.pr.stop()
self.pr.shutdown()
def setParams(self, params):
SCENE_FILE = params["scene_dir"]
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.cameras = {}
cam = VisionSensor("Camera_Shoulder")
self.cameras["Camera_Shoulder"] = {
"handle":
cam,
"id":
1,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"depth":
3,
"focal":
cam.get_resolution()[0] /
np.tan(np.radians(cam.get_perspective_angle())),
"position":
cam.get_position(),
"rotation":
cam.get_quaternion(),
"image_rgb":
np.array(0),
"image_rgbd":
np.array(0),
"depth":
np.ndarray(0)
}
self.grasping_objects = {}
can = Shape("can")
self.grasping_objects["002_master_chef_can"] = {
"handle": can,
"sim_pose": None,
"pred_pose_rgb": None,
"pred_pose_rgbd": None
}
with (open("objects_pcl.pickle", "rb")) as file:
self.object_pcl = pickle.load(file)
self.intrinsics = np.array(
[[
self.cameras["Camera_Shoulder"]["focal"], 0.00000000e+00,
self.cameras["Camera_Shoulder"]["width"] / 2.0
],
[
0.00000000e+00, self.cameras["Camera_Shoulder"]["focal"],
self.cameras["Camera_Shoulder"]["height"] / 2.0
], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
self.arm_ops = {
"MoveToHome": 1,
"MoveToObj": 2,
"CloseGripper": 3,
"OpenGripper": 4
}
self.grasping_iter = 10
self.arm_base = Shape("gen3")
self.arm_target = Dummy("target")
self.gripper = Joint("RG2_openCloseJoint")
def compute(self):
print('SpecificWorker.compute...')
try:
self.pr.step()
# open the arm gripper
self.move_gripper(self.arm_ops["OpenGripper"])
# read arm camera RGB signal
cam = self.cameras["Camera_Shoulder"]
image_float = cam["handle"].capture_rgb()
depth = cam["handle"].capture_depth(in_meters=False)
image = cv2.normalize(src=image_float,
dst=None,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
cam["image_rgb"] = RoboCompCameraRGBDSimple.TImage(
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
image=image.tobytes())
cam["image_rgbd"] = RoboCompCameraRGBDSimple.TImage(
width=cam["width"],
height=cam["height"],
depth=3,
focalx=cam["focal"],
focaly=cam["focal"],
image=image.tobytes())
cam["depth"] = RoboCompCameraRGBDSimple.TDepth(
width=cam["width"],
height=cam["height"],
depthFactor=1.0,
depth=depth.tobytes())
# get objects's poses from simulator
for obj_name in self.grasping_objects.keys():
self.grasping_objects[obj_name][
"sim_pose"] = self.grasping_objects[obj_name][
"handle"].get_pose()
# get objects' poses from RGB
pred_poses = self.objectposeestimationrgb_proxy.getObjectPose(
cam["image_rgb"])
self.visualize_poses(image_float, pred_poses, "rgb_pose.png")
for pose in pred_poses:
if pose.objectname in self.grasping_objects.keys():
obj_trans = [pose.x, pose.y, pose.z]
obj_quat = [pose.qx, pose.qy, pose.qz, pose.qw]
obj_pose = self.process_pose(obj_trans, obj_quat)
self.grasping_objects[
pose.objectname]["pred_pose_rgb"] = obj_pose
# get objects' poses from RGBD
pred_poses = self.objectposeestimationrgbd_proxy.getObjectPose(
cam["image_rgbd"], cam["depth"])
self.visualize_poses(image_float, pred_poses, "rgbd_pose.png")
for pose in pred_poses:
if pose.objectname in self.grasping_objects.keys():
obj_trans = [pose.x, pose.y, pose.z]
obj_quat = [pose.qx, pose.qy, pose.qz, pose.qw]
obj_pose = self.process_pose(obj_trans, obj_quat)
self.grasping_objects[
pose.objectname]["pred_pose_rgbd"] = obj_pose
# create a dummy for arm path planning
approach_dummy = Dummy.create()
approach_dummy.set_name("approach_dummy")
# initialize approach dummy in embedded lua scripts
call_ret = self.pr.script_call(
"initDummy@gen3", vrepConst.sim_scripttype_childscript)
# set object pose for the arm to follow
# NOTE : choose simulator or predicted pose
dest_pose = self.grasping_objects["002_master_chef_can"][
"pred_pose_rgbd"]
dest_pose[
2] += 0.04 # add a small offset along z-axis to grasp the object top
# set dummy pose with the pose of object to be grasped
approach_dummy.set_pose(dest_pose)
# move gen3 arm to the object
self.move_arm(approach_dummy, self.arm_ops["MoveToObj"])
# close the arm gripper
self.move_gripper(self.arm_ops["CloseGripper"])
# change approach dummy pose to the final destination pose
dest_pose[2] += 0.4
approach_dummy.set_pose(dest_pose)
# move gen3 arm to the final destination
self.move_arm(approach_dummy, self.arm_ops["MoveToObj"])
# remove the created approach dummy
approach_dummy.remove()
except Exception as e:
print(e)
return True
def process_pose(self, obj_trans, obj_rot):
# convert an object pose from camera frame to world frame
# define camera pose and z-axis flip matrix
cam_trans = self.cameras["Camera_Shoulder"]["position"]
cam_rot_mat = R.from_quat(self.cameras["Camera_Shoulder"]["rotation"])
z_flip = R.from_matrix(np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]))
# get object position in world coordinates
obj_trans = np.dot(
cam_rot_mat.as_matrix(),
np.dot(z_flip.as_matrix(),
np.array(obj_trans).reshape(-1, )))
final_trans = obj_trans + cam_trans
# get object orientation in world coordinates
obj_rot_mat = R.from_quat(obj_rot)
final_rot_mat = obj_rot_mat * z_flip * cam_rot_mat
final_rot = final_rot_mat.as_quat()
# return final object pose in world coordinates
final_pose = list(final_trans)
final_pose.extend(list(final_rot))
return final_pose
def visualize_poses(self, image, poses, img_name):
# visualize the predicted poses on RGB image
image = np.uint8(image * 255.0)
for pose in poses:
# visualize only defined objects
if pose.objectname not in self.grasping_objects.keys():
continue
obj_pcl = self.object_pcl[pose.objectname]
obj_trans = np.array([pose.x, pose.y, pose.z])
if img_name == "rgb_pose.png":
obj_trans[2] -= 0.2
obj_rot = R.from_quat([pose.qx, pose.qy, pose.qz,
pose.qw]).as_matrix()
proj_pcl = self.vertices_reprojection(obj_pcl, obj_rot, obj_trans,
self.intrinsics)
image = self.draw_pcl(image,
proj_pcl,
r=1,
color=(randint(0, 255), randint(0, 255),
randint(0, 255)))
cv2.imwrite(os.path.join("output", img_name), image)
def vertices_reprojection(self, vertices, r, t, k):
# re-project vertices in pixel space
p = np.matmul(k, np.matmul(r, vertices.T) + t.reshape(-1, 1))
p[0] = p[0] / (p[2] + 1e-5)
p[1] = p[1] / (p[2] + 1e-5)
return p[:2].T
def draw_pcl(self, img, p2ds, r=1, color=(255, 0, 0)):
# draw object point cloud on RGB image
h, w = img.shape[0], img.shape[1]
for pt_2d in p2ds:
pt_2d[0] = np.clip(pt_2d[0], 0, w)
pt_2d[1] = np.clip(pt_2d[1], 0, h)
img = cv2.circle(img, (int(pt_2d[0]), int(pt_2d[1])), r, color, -1)
return img
def move_arm(self, dummy_dest, func_number):
# move arm to destination
# NOTE : this function is using remote lua scripts embedded in the arm
# for better path planning, so make sure to use the correct arm model
call_function = True
init_pose = np.array(
self.arm_target.get_pose(relative_to=self.arm_base))
# loop until the arm reached the object
while True:
# step the simulation
self.pr.step()
# set function index to the desired operation
if call_function:
try:
# call thearded child lua scripts via PyRep
call_ret = self.pr.script_call(
"setFunction@gen3",
vrepConst.sim_scripttype_childscript,
ints=[func_number])
except Exception as e:
print(e)
# get current poses to compare
actual_pose = self.arm_target.get_pose(relative_to=self.arm_base)
object_pose = dummy_dest.get_pose(relative_to=self.arm_base)
# compare poses to check for operation end
pose_diff = np.abs(np.array(actual_pose) - np.array(init_pose))
if call_function and pose_diff[0] > 0.01 or pose_diff[
1] > 0.01 or pose_diff[2] > 0.01:
call_function = False
# check whether the arm reached the target
dest_pose_diff = np.abs(
np.array(actual_pose) - np.array(object_pose))
if dest_pose_diff[0] < 0.015 and dest_pose_diff[
1] < 0.015 and dest_pose_diff[2] < 0.015:
break
def move_gripper(self, func_number):
# open or close the arm gripper
# NOTE : this function is using remote lua scripts embedded in the arm
# for better path planning, so make sure to use the correct arm model
call_function = True
open_percentage = init_position = self.gripper.get_joint_position()
# loop until the gripper is completely open (or closed)
for iter in range(self.grasping_iter):
# step the simulation
self.pr.step()
# set function index to the desired operation
if call_function:
try:
# call thearded child lua scripts via PyRep
call_ret = self.pr.script_call(
"setFunction@gen3",
vrepConst.sim_scripttype_childscript,
ints=[func_number])
except Exception as e:
print(e)
# compare the gripper position to determine whether the gripper moved
if abs(self.gripper.get_joint_position() -
init_position) > 0.005:
call_function = False
# compare the gripper position to determine whether the gripper closed or opened
if not call_function and abs(open_percentage - self.gripper.
get_joint_position()) < 0.003:
break
#actualizamos el porcentaje de apertura
open_percentage = self.gripper.get_joint_position()
