Skip to content

Commit

Permalink
update indyeye, fix moby client msg issue, add requires range
Browse files Browse the repository at this point in the history
  • Loading branch information
Nguyen Pham committed Jul 11, 2024
1 parent e32d2ce commit ad90e36
Show file tree
Hide file tree
Showing 14 changed files with 1,564 additions and 655 deletions.
61 changes: 55 additions & 6 deletions python/build/lib/neuromeka/eye.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,7 @@
EYE_PORT_GRPC = 10511
custom_dat_len = 32 # default custom data length

MAX_POS_LOCATION = 10.0 #10 meters

class EyeCommand:
DETECT = 0 # detect command
Expand All @@ -105,11 +106,13 @@ class DetectKey:
# @class IndyEyeClient
# @brief Rest API client
class IndyEye:
def __init__(self, eye_ip):
# @param pose_unit unit of the end-effector pose, "mm" or "m", default: "mm"
def __init__(self, eye_ip, pose_unit="mm"):
self.session = requests.session()
self.eye_ip = eye_ip
self.url = f"http://{eye_ip}:8088"
self.version = self.get_version()
self.pose_unit = pose_unit.lower()
if float(".".join(self.version.split(".")[:2])) >= 0.5:
# initialize gRPC
self.channel = grpc.insecure_channel('{}:{}'.format(eye_ip, EYE_PORT_GRPC),
Expand All @@ -120,11 +123,37 @@ def __init__(self, eye_ip):
else:
self.run_command = self.run_command_tcp

def get_unit_multiplier(self, pose_unit=None):
if pose_unit is None:
pose_unit = self.pose_unit
if pose_unit in ["m", "meter", "meters"]:
return 1.0
else: #mm unit
return 1000.0

def fix_pose_unit_input(self, pos):
if pos is not None:
new_pos = [p for p in pos]
if len(new_pos) == 6:
for i in range(3):
new_pos[i] = new_pos[i]/self.get_unit_multiplier()
return new_pos
return pos

def fix_pose_unit_output(self, pos):
if pos is not None:
new_pos = [p for p in pos]
if len(new_pos) == 6:
for i in range(3):
new_pos[i] = new_pos[i]*self.get_unit_multiplier("mm")
return new_pos
return pos

##
# @brief indyeye communication function
# @param cmd command id, 0~3
# @param cls index of target class to detect. 0: ALL, 1~: index of class
# @param pose_cmd end-effector pose, [x,y,z,u,v,w] (unit: m, deg)
# @param pose_cmd end-effector pose, [x,y,z,u,v,w] (unit: self.pose_unit, deg)
# @param robot_ip ip address of robot, for multi-robot use
def run_command_grpc(self, cmd, cls=0, pose_cmd=None, robot_ip=None, **kwargs):
self.request_id += 1
Expand All @@ -136,7 +165,9 @@ def run_command_grpc(self, cmd, cls=0, pose_cmd=None, robot_ip=None, **kwargs):
sdict = {'id': self.request_id, 'cls': int(cls)}
if cmd == EyeCommand.DETECT:
if pose_cmd is not None:
sdict['pose_cmd'] = pose_cmd
sdict['pose_cmd'] = self.fix_pose_unit_input(pose_cmd)
if max(sdict['pose_cmd'][:3]) >= MAX_POS_LOCATION or min(sdict['pose_cmd'][:3]) <= MAX_POS_LOCATION * -1:
raise(ValueError("Input pose exceeds {}m".format(MAX_POS_LOCATION)))
if robot_ip is not None:
sdict['robot_ip'] = robot_ip
resp = self.stub.Detect(EyeTask_pb2.DetectRequest(**sdict))
Expand All @@ -146,7 +177,7 @@ def run_command_grpc(self, cmd, cls=0, pose_cmd=None, robot_ip=None, **kwargs):
resp = self.stub.Retrieve(EyeTask_pb2.RetrieveRequest(**sdict))
else:
raise(NotImplementedError("Unknown command {}".format(cmd)))
return {DetectKey.DETECTED: resp.detected,
result = {DetectKey.DETECTED: resp.detected,
DetectKey.PASSED: resp.passed,
DetectKey.CLASS: resp.cls,
DetectKey.REF_TO_END_TOOL: resp.tar_ee_pose,
Expand All @@ -155,12 +186,21 @@ def run_command_grpc(self, cmd, cls=0, pose_cmd=None, robot_ip=None, **kwargs):
DetectKey.TOOL_INDEX: resp.tool_idx,
DetectKey.ERROR_STATE: resp.error_state,
DetectKey.ERROR_MODULE: resp.error_module}

if DetectKey.REF_TO_END_TOOL in result:
result[DetectKey.REF_TO_END_TOOL] = self.fix_pose_unit_output(result[DetectKey.REF_TO_END_TOOL])
if DetectKey.REF_TO_PICK_POINT in result:
result[DetectKey.REF_TO_PICK_POINT] = self.fix_pose_unit_output(result[DetectKey.REF_TO_PICK_POINT])
if DetectKey.REF_TO_OBJECT in result:
result[DetectKey.REF_TO_OBJECT] = self.fix_pose_unit_output(result[DetectKey.REF_TO_OBJECT])

return result

##
# @brief indyeye communication function
# @param cmd command id, 0~3
# @param cls index of target class to detect. 0: ALL, 1~: index of class
# @param pose_cmd end-effector pose, [x,y,z,u,v,w] (unit: m, deg)
# @param pose_cmd end-effector pose, [x,y,z,u,v,w] (unit: self.pose_unit, deg)
# @param robot_ip ip address of robot, for multi-robot use
def run_command_tcp(self, cmd, cls, pose_cmd=None, robot_ip=None, **kwargs):
sock = socket.socket(socket.AF_INET,
Expand All @@ -170,7 +210,9 @@ def run_command_tcp(self, cmd, cls, pose_cmd=None, robot_ip=None, **kwargs):
sock.connect((self.eye_ip, EYE_PORT_TCP))
sdict = {'command': int(cmd), 'class_tar': int(cls), }
if pose_cmd is not None:
sdict['pose_cmd'] = pose_cmd
sdict['pose_cmd'] = self.fix_pose_unit_input(pose_cmd)
if max(sdict['pose_cmd'][:3]) >= MAX_POS_LOCATION or min(sdict['pose_cmd'][:3]) <= MAX_POS_LOCATION * -1:
raise(ValueError("Input pose exceeds {}m".format(MAX_POS_LOCATION)))
if robot_ip is not None:
sdict['robot_ip'] = robot_ip
sdict.update(kwargs)
Expand All @@ -195,6 +237,13 @@ def run_command_tcp(self, cmd, cls, pose_cmd=None, robot_ip=None, **kwargs):
DetectKey.TOOL_INDEX: rdict['tool_idx'],
DetectKey.ERROR_STATE: rdict['STATE']!=0,
DetectKey.ERROR_MODULE: None}

if DetectKey.REF_TO_END_TOOL in rdict:
rdict[DetectKey.REF_TO_END_TOOL] = self.fix_pose_unit_output(rdict[DetectKey.REF_TO_END_TOOL])
if DetectKey.REF_TO_PICK_POINT in rdict:
rdict[DetectKey.REF_TO_PICK_POINT] = self.fix_pose_unit_output(rdict[DetectKey.REF_TO_PICK_POINT])
if DetectKey.REF_TO_OBJECT in rdict:
rdict[DetectKey.REF_TO_OBJECT] = self.fix_pose_unit_output(rdict[DetectKey.REF_TO_OBJECT])
else:
raise(NotImplementedError("Unknown command {}".format(cmd)))
return rdict
Expand Down
98 changes: 67 additions & 31 deletions python/build/lib/neuromeka/indydcp3.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@
from neuromeka.common import *
from neuromeka.enums import *


import grpc
from google.protobuf import json_format
from google.protobuf.json_format import ParseDict
Expand Down Expand Up @@ -76,6 +75,7 @@ def get_robot_data(self):
including_default_value_fields=True,
preserving_proto_field_name=True,
use_integers_for_enums=True)

def get_control_data(self):
return self.get_robot_data()

Expand Down Expand Up @@ -491,11 +491,11 @@ def movel(self, ttarget,
use_integers_for_enums=True)

def movel_time(self, ttarget,
blending_type=BlendingType.NONE,
base_type=TaskBaseType.ABSOLUTE,
blending_radius=0.0,
move_time=5.0,
post_condition=PostCondition()) -> dict:
blending_type=BlendingType.NONE,
base_type=TaskBaseType.ABSOLUTE,
blending_radius=0.0,
move_time=5.0,
post_condition=PostCondition()) -> dict:
"""
tstart = [mm, mm, mm, deg, deg, deg]
ttarget = [mm, mm, mm, deg, deg, deg]
Expand Down Expand Up @@ -587,15 +587,13 @@ def movec(self, tpos0, tpos1,
def move_home(self):
home_pos = self.get_home_pos()['jpos']
self.movej(home_pos,
blending_type=BlendingType.NONE,
base_type=JointBaseType.ABSOLUTE,
blending_radius=0.0,
vel_ratio=Limits.JogVelRatioDefault,
acc_ratio=Limits.JogAccRatioDefault,
post_condition=PostCondition(),
teaching_mode=False)


blending_type=BlendingType.NONE,
base_type=JointBaseType.ABSOLUTE,
blending_radius=0.0,
vel_ratio=Limits.JogVelRatioDefault,
acc_ratio=Limits.JogAccRatioDefault,
post_condition=PostCondition(),
teaching_mode=False)

############################
# Motion Control (Teleoperation)
Expand All @@ -609,7 +607,8 @@ def start_teleop(self, method):
TELE_JOINT_ABSOLUTE = 10
TELE_JOINT_RELATIVE = 11
"""
response = self.control.StartTeleOp(control_msgs.TeleOpState(mode=control_msgs.TeleMode.TELE_RAW, method=method))
response = self.control.StartTeleOp(
control_msgs.TeleOpState(mode=control_msgs.TeleMode.TELE_RAW, method=method))
return json_format.MessageToDict(response,
including_default_value_fields=True,
preserving_proto_field_name=True,
Expand Down Expand Up @@ -655,7 +654,7 @@ def movetelel_abs(self, tpos, vel_ratio=1.0, acc_ratio=1.0):
jpos = [mm, mm, mm, deg, deg, deg]
"""
response = self.control.MoveTeleL(control_msgs.MoveTeleLReq(tpos=tpos, vel_ratio=vel_ratio, acc_ratio=acc_ratio,
method=control_msgs.TELE_TASK_ABSOLUTE))
method=control_msgs.TELE_TASK_ABSOLUTE))
return json_format.MessageToDict(response,
including_default_value_fields=True,
preserving_proto_field_name=True,
Expand All @@ -667,13 +666,12 @@ def movetelel_rel(self, tpos, vel_ratio=1.0, acc_ratio=1.0):
jpos = [mm, mm, mm, deg, deg, deg]
"""
response = self.control.MoveTeleL(control_msgs.MoveTeleLReq(tpos=tpos, vel_ratio=vel_ratio, acc_ratio=acc_ratio,
method=control_msgs.TELE_TASK_RELATIVE))
method=control_msgs.TELE_TASK_RELATIVE))
return json_format.MessageToDict(response,
including_default_value_fields=True,
preserving_proto_field_name=True,
use_integers_for_enums=True)


############################
# Control - Additional
############################
Expand Down Expand Up @@ -748,7 +746,6 @@ def calculate_relative_pose(self, start_pos, end_pos,
preserving_proto_field_name=True,
use_integers_for_enums=True)


def calculate_current_pose_rel(self, current_pos, relative_pos,
base_type=TaskBaseType.ABSOLUTE):
"""
Expand Down Expand Up @@ -823,7 +820,6 @@ def set_speed_ratio(self, speed_ratio: int):
preserving_proto_field_name=True,
use_integers_for_enums=True)


############################
# Variables
############################
Expand Down Expand Up @@ -997,7 +993,6 @@ def set_tpos_variable(self, tpos_variables: list):
preserving_proto_field_name=True,
use_integers_for_enums=True)


############################
# Config
############################
Expand Down Expand Up @@ -1025,7 +1020,6 @@ def set_home_pos(self, home_jpos: list):
preserving_proto_field_name=True,
use_integers_for_enums=True)


def get_ref_frame(self):
"""
Reference frame
Expand Down Expand Up @@ -1077,7 +1071,6 @@ def set_tool_frame(self, fpos: list):
preserving_proto_field_name=True,
use_integers_for_enums=True)


def get_friction_comp(self):
"""
Friction Compensation Set:
Expand Down Expand Up @@ -1113,7 +1106,6 @@ def set_friction_comp(self, control_comp: bool, control_comp_levels: list,
preserving_proto_field_name=True,
use_integers_for_enums=True)


def get_tool_property(self):
"""
Tool Properties:
Expand Down Expand Up @@ -1290,7 +1282,6 @@ def set_safety_limits(self, power_limit: float, power_limit_ratio: float,
preserving_proto_field_name=True,
use_integers_for_enums=True)


############################
# IndySDK related
############################
Expand Down Expand Up @@ -1336,6 +1327,55 @@ def get_custom_control_mode(self):
preserving_proto_field_name=True,
use_integers_for_enums=True)

def get_custom_control_gain(self):
"""
Custom Control Gain
gain0 -> float[6]
gain1 -> float[6]
gain2 -> float[6]
gain3 -> float[6]
gain4 -> float[6]
gain5 -> float[6]
gain6 -> float[6]
gain7 -> float[6]
gain8 -> float[6]
gain9 -> float[6]
"""
response = self.config.GetCustomControlGain(common_msgs.Empty())
return json_format.MessageToDict(response,
including_default_value_fields=True,
preserving_proto_field_name=True,
use_integers_for_enums=True)

def set_custom_control_gain2(self, gain0, gain1):
return self._set_custom_control_gain(gain0, gain1, *[([0] * 6) for _ in range(8)])

def set_custom_control_gain3(self, gain0, gain1, gain2):
return self._set_custom_control_gain(gain0, gain1, gain2, *[([0] * 6) for _ in range(7)])

def set_custom_control_gain6(self, gain0, gain1, gain2, gain3, gain4, gain5):
return self._set_custom_control_gain(gain0, gain1, gain2, gain3, gain4, gain5, *[([0] * 6) for _ in range(4)])

def set_custom_control_gain(self, gain0, gain1, gain2, gain3, gain4, gain5, gain6, gain7, gain8, gain9):
return self._set_custom_control_gain(gain0, gain1, gain2, gain3, gain4, gain5, gain6, gain7, gain8, gain9)

def _set_custom_control_gain(self, *gains):
"""
Private method to set custom control gains with a variable number of gain arrays.
Args:
*gains: Up to 10 lists of gain values. Each gain should be a list of floats.
"""
response = self.config.SetCustomControlGain(config_msgs.CustomGainSet(
gain0=list(gains[0]), gain1=list(gains[1]), gain2=list(gains[2]), gain3=list(gains[3]),
gain4=list(gains[4]), gain5=list(gains[5]), gain6=list(gains[6]), gain7=list(gains[7]),
gain8=list(gains[8]), gain9=list(gains[9])
))
return json_format.MessageToDict(response,
including_default_value_fields=True,
preserving_proto_field_name=True,
use_integers_for_enums=True)

############################
# Uility functions
############################
Expand Down Expand Up @@ -1495,7 +1535,6 @@ def get_control_info(self):
preserving_proto_field_name=True,
use_integers_for_enums=True)


def check_aproach_retract_valid(self, tpos, init_jpos, pre_tpos, post_tpos):
"""
Check aproach retract valid
Expand Down Expand Up @@ -1529,7 +1568,6 @@ def get_pallet_point_list(self, tpos, jpos, pre_tpos, post_tpos, pallet_pattern,
preserving_proto_field_name=True,
use_integers_for_enums=True)


def play_tuning_program(self, prog_name: str = '', prog_idx: int = -1,
tuning_space=common_msgs.TUNE_ALL, precision=common_msgs.HIGH_PRECISION,
vel_level_max=9):
Expand Down Expand Up @@ -1600,8 +1638,6 @@ def get_di_config_list(self):
preserving_proto_field_name=True,
use_integers_for_enums=True)



def set_auto_servo_Off(self, enable: bool, time: float):
"""
Auto Servo-Off Config
Expand Down Expand Up @@ -1689,4 +1725,4 @@ def get_io_data(self):
return json_format.MessageToDict(response,
including_default_value_fields=True,
preserving_proto_field_name=True,
use_integers_for_enums=True)
use_integers_for_enums=True)
Loading

0 comments on commit ad90e36

Please sign in to comment.