diff --git a/python/build/lib/neuromeka/eye.py b/python/build/lib/neuromeka/eye.py index 65d7b4c..6c5f6ad 100644 --- a/python/build/lib/neuromeka/eye.py +++ b/python/build/lib/neuromeka/eye.py @@ -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 @@ -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), @@ -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 @@ -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)) @@ -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, @@ -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, @@ -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) @@ -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 diff --git a/python/build/lib/neuromeka/indydcp3.py b/python/build/lib/neuromeka/indydcp3.py index 27f52bd..7ce15e0 100644 --- a/python/build/lib/neuromeka/indydcp3.py +++ b/python/build/lib/neuromeka/indydcp3.py @@ -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 @@ -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() @@ -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] @@ -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) @@ -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, @@ -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, @@ -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 ############################ @@ -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): """ @@ -823,7 +820,6 @@ def set_speed_ratio(self, speed_ratio: int): preserving_proto_field_name=True, use_integers_for_enums=True) - ############################ # Variables ############################ @@ -997,7 +993,6 @@ def set_tpos_variable(self, tpos_variables: list): preserving_proto_field_name=True, use_integers_for_enums=True) - ############################ # Config ############################ @@ -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 @@ -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: @@ -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: @@ -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 ############################ @@ -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 ############################ @@ -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 @@ -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): @@ -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 @@ -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) \ No newline at end of file + use_integers_for_enums=True) diff --git a/python/build/lib/neuromeka/moby.py b/python/build/lib/neuromeka/moby.py index 3785141..73a7a3e 100644 --- a/python/build/lib/neuromeka/moby.py +++ b/python/build/lib/neuromeka/moby.py @@ -3,9 +3,9 @@ import grpc, time - MOBY_PORT = 20200 + class MobyClient: def __init__(self, ip_addr): moby_channel = grpc.insecure_channel("{}:{}".format(ip_addr, MOBY_PORT)) @@ -32,7 +32,7 @@ def get_moby_state(self): 7: "MOVING", 16: "TELE_OP", } - state = self.__moby_stub.GetMobyState(Empty()) + state = self.__moby_stub.GetMobyState(common_msgs.Empty()) moby_state = state_dict.get(state.status, "(UNKNOWN STATE CODE)") b_ready = state.is_ready b_enable = state.is_enable @@ -58,21 +58,21 @@ def get_moby_error_state(self): 0x100: "HW_CONNECTION_LOST", } - err = self.__moby_stub.GetMobyErrorState(Empty()) + err = self.__moby_stub.GetMobyErrorState(common_msgs.Empty()) return [error_dict.get(err.errorState, "(UNKNOWN ERROR CODE)"), err.errorIndex1, err.errorIndex2, err.errorIndex3] @Utils.exception_handler def recover(self): - return self.__moby_stub.Recover(Empty()) + return self.__moby_stub.Recover(common_msgs.Empty()) @Utils.exception_handler def get_moby_pose(self): """ Get Moby pose (m): [Px, Py, Pw] """ - pose = self.__moby_stub.GetMobyPose(Empty()) + pose = self.__moby_stub.GetMobyPose(common_msgs.Empty()) return [pose.px, pose.py, pose.pw] @Utils.exception_handler @@ -80,7 +80,7 @@ def get_moby_vel(self): """ Get Moby velocity (m/s): [Vx, Vy, Vw] """ - vel = self.__moby_stub.GetMobyVel(Empty()) + vel = self.__moby_stub.GetMobyVel(common_msgs.Empty()) return [vel.vx, vel.vy, vel.vw] @Utils.exception_handler @@ -88,30 +88,30 @@ def reset_moby_pose(self): """ Reset Moby pose """ - return self.__moby_stub.ResetMobyPose(Empty()) + return self.__moby_stub.ResetMobyPose(common_msgs.Empty()) @Utils.exception_handler def get_rotation_angle(self): """ Get rotation angle (deg): [fl, fr, bl, br] """ - val = self.__moby_stub.GetRotationAngleDeg(Empty()) - return {'fl': val.fl, 'fr': val.fr, 'bl':val.bl, 'br':val.br} + val = self.__moby_stub.GetRotationAngleDeg(common_msgs.Empty()) + return {'fl': val.fl, 'fr': val.fr, 'bl': val.bl, 'br': val.br} @Utils.exception_handler def get_drive_speed(self): """ Get drive speed (m/s): [fl, fr, bl, br] """ - val = self.__moby_stub.GetDriveSpeed(Empty()) - return {'fl': val.fl, 'fr': val.fr, 'bl':val.bl, 'br':val.br} + val = self.__moby_stub.GetDriveSpeed(common_msgs.Empty()) + return {'fl': val.fl, 'fr': val.fr, 'bl': val.bl, 'br': val.br} @Utils.exception_handler def get_target_vel(self): """ Get Moby's target velocity """ - target = self.__moby_stub.GetTargetVel(Empty()) + target = self.__moby_stub.GetTargetVel(common_msgs.Empty()) return [target.vx, target.vy, target.vw] @Utils.exception_handler @@ -120,22 +120,22 @@ def get_zero(self): Get rotation's zero position (encoder count) [fl, fr, bl, br] """ - val = self.__moby_stub.GetRotationZeroCount(Empty()) - return {'fl': val.fl, 'fr': val.fr, 'bl':val.bl, 'br':val.br} + val = self.__moby_stub.GetRotationZeroCount(common_msgs.Empty()) + return {'fl': val.fl, 'fr': val.fr, 'bl': val.bl, 'br': val.br} @Utils.exception_handler def get_gyro_data(self): """ Get Gyro sensor data (yaw, yaw rate) """ - return self.__moby_stub.GetGyroData(Empty()).val + return self.__moby_stub.GetGyroData(common_msgs.Empty()).val @Utils.exception_handler def get_imu_data(self): """ Get Full IMU sensor data """ - data = self.__moby_stub.GetGyroFullData(Empty()) + data = self.__moby_stub.GetGyroFullData(common_msgs.Empty()) angle = [data.angleX, data.angleY, data.angleZ] vel = [data.angleVelX, data.angleVelY, data.angleVelZ] acc = [data.linAccX, data.linAccY, data.linAccZ] @@ -146,23 +146,29 @@ def reset_gyro(self): """ Reset gyro sensor """ - return self.__moby_stub.ResetGyroSensor(Empty()) + return self.__moby_stub.ResetGyroSensor(common_msgs.Empty()) @Utils.exception_handler def use_gyro_for_odom(self, use_gyro): """ Use gyro sensor for odometry calculation """ - return self.__moby_stub.UseGyroForOdom(BoolVal(val=use_gyro)) + return self.__moby_stub.UseGyroForOdom(moby_msgs.BoolVal(val=use_gyro)) @Utils.exception_handler def get_us_data(self): """ Get US sensor data """ - value = self.__moby_stub.GetUSSensorData(Empty()) - return {'front_left1': value.us_front_left1, 'front_left2': value.us_front_left2, 'front_left3': value.us_front_left3, 'front_ground': value.us_front_ground, 'front_right1': value.us_front_right1, 'front_right2': value.us_front_right2, 'front_right3': value.us_front_right3, 'front_right4': value.us_front_right4, 'back_right1': value.us_back_right1, 'back_right2': value.us_back_right2, 'back_right3': value.us_back_right3, 'back_ground': value.us_back_ground, 'back_left1': value.us_back_left1, 'back_left2': value.us_back_left2, 'back_left3': value.us_back_left3, 'back_left4': value.us_back_left4} - + value = self.__moby_stub.GetUSSensorData(common_msgs.Empty()) + return {'front_left1': value.us_front_left1, 'front_left2': value.us_front_left2, + 'front_left3': value.us_front_left3, 'front_ground': value.us_front_ground, + 'front_right1': value.us_front_right1, 'front_right2': value.us_front_right2, + 'front_right3': value.us_front_right3, 'front_right4': value.us_front_right4, + 'back_right1': value.us_back_right1, 'back_right2': value.us_back_right2, + 'back_right3': value.us_back_right3, 'back_ground': value.us_back_ground, + 'back_left1': value.us_back_left1, 'back_left2': value.us_back_left2, 'back_left3': value.us_back_left3, + 'back_left4': value.us_back_left4} @Utils.exception_handler def get_bms(self): @@ -184,7 +190,7 @@ def get_bms(self): 'Remain Capacity Ah', 'Remain Capacity Wh' 'Temperature-(1~3)', 'Cell Voltage-(1~13)' """ - value = self.__moby_stub.GetBMSData(Empty()) + value = self.__moby_stub.GetBMSData(common_msgs.Empty()) return {'BMS status-1': value.bms_status[0] / 10, 'BMS status-2': value.bms_status[1] / 10, 'Pack voltage-1': value.pack_volt[0] / 100, 'Pack voltage-2': value.pack_volt[1] / 100, 'Battery Voltage-1': value.battery_volt[0] / 100, 'Battery Voltage-2': value.battery_volt[1] / 100, @@ -211,35 +217,35 @@ def set_target_vel(self, vx, vy, vw): """ Drive control """ - return self.__moby_stub.SetStepControl(TargetVel(vx=vx, vy=vy, vw=vw)) + return self.__moby_stub.SetStepControl(moby_msgs.TargetVel(vx=vx, vy=vy, vw=vw)) @Utils.exception_handler def stop_motion(self): """ Stop Moby motion """ - return self.__moby_stub.StopMotion(Empty()) + return self.__moby_stub.StopMotion(common_msgs.Empty()) @Utils.exception_handler def go_straight(self): """ Go straight (zero rotation) """ - return self.__moby_stub.SetRotationAngleDeg(SwerveDoubles(fl=0, fr=0, bl=0, br=0)) + return self.__moby_stub.SetRotationAngleDeg(moby_msgs.SwerveDoubles(fl=0, fr=0, bl=0, br=0)) @Utils.exception_handler def move_rotation_deg(self, fl, fr, bl, br): """ Rotation control (target angle degree) """ - return self.__moby_stub.SetRotationAngleDeg(SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) + return self.__moby_stub.SetRotationAngleDeg(moby_msgs.SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) @Utils.exception_handler def move_driving_mps(self, fl, fr, bl, br): """ Driving control (target speed m/s) """ - return self.__moby_stub.DriveWheel(SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) + return self.__moby_stub.DriveWheel(moby_msgs.SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) ############################ # Set Moby parameters @@ -250,14 +256,14 @@ def set_zero_as_current(self): """ Set current roation position as zero """ - return self.__moby_stub.SetZeroPosAsCurrentPos(Empty()) + return self.__moby_stub.SetZeroPosAsCurrentPos(common_msgs.Empty()) @Utils.exception_handler def set_rotation_vel_acc(self, vel, acc): """ Set rotation maximum velocity, acceleration """ - return self.__moby_stub.SetRotationVelAcc(DoubleVals(val=[vel, acc])) + return self.__moby_stub.SetRotationVelAcc(moby_msgs.DoubleVals(val=[vel, acc])) @Utils.exception_handler def set_rotation_interpolator(self, val): @@ -268,21 +274,21 @@ def set_rotation_interpolator(self, val): 2: Velocity interpolator 3: Trapezoidal interpolator """ - return self.__moby_stub.SetRotationInterpolator(IntVal(val=val)) + return self.__moby_stub.SetRotationInterpolator(moby_msgs.IntVal(val=val)) @Utils.exception_handler def set_drive_acc_dec(self, acc, dec): """ Set drive acc dec """ - return self.__moby_stub.SetDriveAccDec(DoubleVals(val=[acc, dec])) + return self.__moby_stub.SetDriveAccDec(moby_msgs.DoubleVals(val=[acc, dec])) @Utils.exception_handler def set_drive_interpolator_on_off(self, on): """ Set drive interpolator On Off """ - return self.__moby_stub.SetDriveInterpolatorOnOff(BoolVal(val=on)) + return self.__moby_stub.SetDriveInterpolatorOnOff(moby_msgs.BoolVal(val=on)) @Utils.exception_handler def set_rotation_controller_type(self, val): @@ -292,69 +298,68 @@ def set_rotation_controller_type(self, val): 1: SIMPLEPID_POS_CONTROLLER, 2: SIMPLEPID_VEL_CONTROLLER """ - return self.__moby_stub.SetRotationControllerType(IntVal(val=val)) + return self.__moby_stub.SetRotationControllerType(moby_msgs.IntVal(val=val)) @Utils.exception_handler def set_rotation_gain(self, index, k, kv, kp): """ Set Rotation Control Gain """ - return self.__moby_stub.SetControlParam(RotationGain(idx=index, k=k, kv=kv, kp=kp)) + return self.__moby_stub.SetControlParam(moby_msgs.RotationGain(idx=index, k=k, kv=kv, kp=kp)) @Utils.exception_handler def get_rotation_gain(self, index): """ Get Rotation Control Gain (k, kv, kp) """ - val = self.__moby_stub.GetControlParam(IntVal(val=index)) - return {'k': val.k, 'kv':val.kv, 'kp':val.kp} - + val = self.__moby_stub.GetControlParam(moby_msgs.IntVal(val=index)) + return {'k': val.k, 'kv': val.kv, 'kp': val.kp} + @Utils.exception_handler def set_kinematics_forced(self, onoff): """ Set Kinematics Forced( on=true, off=false ) """ - self.__moby_stub.SetForceKinematics(BoolVal(val=onoff)) + self.__moby_stub.SetForceKinematics(moby_msgs.BoolVal(val=onoff)) @Utils.exception_handler def get_kinematics_forced(self): """ Get Kinematics Forced( on=true, off=false ) """ - val = self.__moby_stub.GetForceKinematics(Empty()) + val = self.__moby_stub.GetForceKinematics(common_msgs.Empty()) return val ############################ # Moby-Agri related commands ############################ - + @Utils.exception_handler def turn_light(self, on): - return self.__moby_stub.TurnLightOnOff(BoolVal(val=on)) - + return self.__moby_stub.TurnLightOnOff(moby_msgs.BoolVal(val=on)) + @Utils.exception_handler def turn_buzzer(self, on): - return self.__moby_stub.TurnBuzzOnOff(BoolVal(val=on)) + return self.__moby_stub.TurnBuzzOnOff(moby_msgs.BoolVal(val=on)) @Utils.exception_handler def pause_bumper(self, on): - return self.__moby_stub.PauseBumper(BoolVal(val=on)) + return self.__moby_stub.PauseBumper(moby_msgs.BoolVal(val=on)) ############################ # Moby Data logging ############################ - + @Utils.exception_handler def start_rt_logging(self): """ Start RT logging """ - return self.__moby_stub.StartRTLogging(Empty()) - + return self.__moby_stub.StartRTLogging(common_msgs.Empty()) + @Utils.exception_handler def end_rt_logging(self): """ End RT logging """ - return self.__moby_stub.EndRTLogging(Empty()) - + return self.__moby_stub.EndRTLogging(common_msgs.Empty()) diff --git a/python/dist/neuromeka-3.2.0.5-py3-none-any.whl b/python/dist/neuromeka-3.2.0.5-py3-none-any.whl deleted file mode 100644 index 8dbdd9c..0000000 Binary files a/python/dist/neuromeka-3.2.0.5-py3-none-any.whl and /dev/null differ diff --git a/python/dist/neuromeka-3.2.0.5.tar.gz b/python/dist/neuromeka-3.2.0.5.tar.gz deleted file mode 100644 index 93c234c..0000000 Binary files a/python/dist/neuromeka-3.2.0.5.tar.gz and /dev/null differ diff --git a/python/dist/neuromeka-3.2.0.7-py3-none-any.whl b/python/dist/neuromeka-3.2.0.7-py3-none-any.whl new file mode 100644 index 0000000..5a31b2d Binary files /dev/null and b/python/dist/neuromeka-3.2.0.7-py3-none-any.whl differ diff --git a/python/examples/indydcp3_example.ipynb b/python/examples/indydcp3_example.ipynb index c3a36ef..5c3fd96 100644 --- a/python/examples/indydcp3_example.ipynb +++ b/python/examples/indydcp3_example.ipynb @@ -12,14 +12,16 @@ }, { "cell_type": "code", + "execution_count": 37, "id": "aef98a24-721b-46c2-8f43-bf0f26ce6fbf", "metadata": { - "scrolled": true, "ExecuteTime": { "end_time": "2024-07-10T16:30:24.443908Z", "start_time": "2024-07-10T16:30:24.437671Z" - } + }, + "scrolled": true }, + "outputs": [], "source": [ "try: \n", " # Use installed PyPI package\n", @@ -32,9 +34,7 @@ "\n", "step_ip = \"192.168.1.29\"\n", "indy = IndyDCP3(step_ip)" - ], - "outputs": [], - "execution_count": 2 + ] }, { "cell_type": "markdown", @@ -53,6 +53,7 @@ }, { "cell_type": "code", + "execution_count": 38, "id": "23f2a13c-0d21-48ce-8c46-654d86ad41cc", "metadata": { "ExecuteTime": { @@ -60,132 +61,301 @@ "start_time": "2024-07-10T16:30:31.310405Z" } }, - "source": [ - "indy.get_robot_data()" - ], "outputs": [ { "data": { "text/plain": [ - "{'running_hours': 11,\n", - " 'running_mins': 46,\n", - " 'running_secs': 59,\n", + "{'running_hours': 28,\n", + " 'running_mins': 42,\n", + " 'running_secs': 33,\n", " 'op_state': 5,\n", " 'sim_mode': True,\n", - " 'q': [-4.539805e-05,\n", - " -0.0030416693,\n", - " -90.03273,\n", - " 0.012726736,\n", - " -89.99929,\n", - " 5.940101e-14],\n", - " 'qdot': [2.5558418e-17,\n", - " -7.577818e-16,\n", - " 8.056228e-15,\n", - " 2.8421964e-15,\n", - " -4.5251427e-15,\n", - " -5.7894102e-18],\n", - " 'p': [349.8841, -186.44963, 521.74084, 179.98727, -0.035066623, 179.99995],\n", - " 'pdot': [1.7098077e-14,\n", - " 1.1468175e-14,\n", - " 3.550126e-14,\n", - " -2.842181e-15,\n", - " -2.7732975e-15,\n", - " -3.2933905e-17],\n", + " 'q': [90.84072,\n", + " -0.98125404,\n", + " -90.00027,\n", + " -0.0003313501,\n", + " -89.0192,\n", + " 1.3514172e-05],\n", + " 'qdot': [1.677157e-08,\n", + " -5.1942944e-08,\n", + " -1.2315095e-08,\n", + " 5.2022227e-09,\n", + " 1.505422e-09,\n", + " -4.2631484e-12],\n", + " 'p': [181.23349, 360.35016, 515.93964, 0.00033130165, -179.99928, 90.84071],\n", + " 'pdot': [-1.2839568e-07,\n", + " 2.0390753e-07,\n", + " -4.1606904e-07,\n", + " -5.2012403e-09,\n", + " 6.275252e-08,\n", + " -1.6687157e-08],\n", " 'ref_frame': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", " 'tool_frame': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}" ] }, - "execution_count": 3, + "execution_count": 38, "metadata": {}, "output_type": "execute_result" } ], - "execution_count": 3 + "source": [ + "indy.get_robot_data()" + ] }, { "cell_type": "code", + "execution_count": 39, "id": "1c5afbbe-7f27-4ed1-80dc-eb59a68bbd42", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "5" + ] + }, + "execution_count": 39, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import OpState\n", "OpState.IDLE" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 40, "id": "49b0956c-99f2-4be3-aba8-e7df6c6f92c5", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'q': [90.84072, -0.98125404, -90.00027, -0.00033135, -89.0192, 1.3514172e-05],\n", + " 'qdot': [1.6664323e-08,\n", + " -5.16412e-08,\n", + " -1.2247555e-08,\n", + " 5.1724673e-09,\n", + " 1.4937851e-09,\n", + " -4.2277358e-12],\n", + " 'qddot': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'qdes': [90.84072,\n", + " -0.9812542,\n", + " -90.00027,\n", + " -0.00033133337,\n", + " -89.0192,\n", + " 1.3514159e-05],\n", + " 'qdotdes': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'qddotdes': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'p': [181.23349, 360.35016, 515.93964, 0.00033130156, -179.99928, 90.84071],\n", + " 'pdot': [-1.275895e-07,\n", + " 2.0266407e-07,\n", + " -4.1367798e-07,\n", + " -5.1714912e-09,\n", + " 6.2394875e-08,\n", + " -1.6580382e-08],\n", + " 'pddot': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'pdes': [181.23349,\n", + " 360.35016,\n", + " 515.93964,\n", + " 0.00033128491,\n", + " -179.99928,\n", + " 90.84071],\n", + " 'pdotdes': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'pddotdes': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'tau': [-2.9090744e-10,\n", + " 21.700241,\n", + " 20.771187,\n", + " -6.261608,\n", + " -0.001489497,\n", + " 2.4171255e-08],\n", + " 'tau_act': [-2.9094768e-10,\n", + " 21.700241,\n", + " 20.771187,\n", + " -6.261608,\n", + " -0.001489497,\n", + " 2.4171255e-08],\n", + " 'tau_ext': [-2.2502633e-11,\n", + " 7.283191e-11,\n", + " 1.7572341e-11,\n", + " -7.34606e-12,\n", + " -2.0740026e-12,\n", + " 5.462812e-15]}" + ] + }, + "execution_count": 40, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_control_state()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 41, "id": "7bee76db-e0e3-46e1-af9d-eccccdc42ebf", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'traj_progress': 100,\n", + " 'speed_ratio': 100,\n", + " 'remain_distance': 0.028216828,\n", + " 'cur_traj_progress': 100,\n", + " 'traj_state': 0,\n", + " 'is_in_motion': False,\n", + " 'is_target_reached': False,\n", + " 'is_pausing': False,\n", + " 'is_stopping': False,\n", + " 'has_motion': False,\n", + " 'motion_id': 0,\n", + " 'motion_queue_size': 0}" + ] + }, + "execution_count": 41, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_motion_data()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 42, "id": "f0ec5ff8-f2ba-4a52-b977-d03f7c4631d6", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "4" + ] + }, + "execution_count": 42, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import TrajState\n", "TrajState.ACC" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 43, "id": "3e6992ea-5f95-4d9d-beb8-98294d338f14", "metadata": { "scrolled": true }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'status_codes': ['0x1237', '0x1237', '0x1237', '0x1237', '0x1237', '0x1237'],\n", + " 'temperatures': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n", + " 'voltages': [48.0, 48.0, 48.0, 48.0, 48.0, 48.0],\n", + " 'currents': [-2.870889e-10,\n", + " 21.700241,\n", + " 20.771187,\n", + " -6.261608,\n", + " -0.0014894971,\n", + " 2.4171333e-08],\n", + " 'servo_actives': [True, True, True, True, True, True],\n", + " 'brake_actives': [False, False, False, False, False, False]}" + ] + }, + "execution_count": 43, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_servo_data()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 44, "id": "5a145b9e-b6dd-49a9-96d9-1e800239fa25", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'i_args': [0],\n", + " 'f_args': [0.0],\n", + " 'violation_code': '0',\n", + " 'j_index': 0,\n", + " 'violation_str': ''}" + ] + }, + "execution_count": 44, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_violation_data()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 45, "id": "f370e0b1-1ebe-400c-8844-5906c2701860", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'program_name': 'ProgramScripts/',\n", + " 'program_state': 0,\n", + " 'cmd_id': 0,\n", + " 'sub_cmd_id': 0,\n", + " 'running_hours': 0,\n", + " 'running_mins': 0,\n", + " 'running_secs': 0,\n", + " 'program_alarm': '',\n", + " 'program_annotation': ''}" + ] + }, + "execution_count": 45, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_program_data()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 46, "id": "cb22d2e4-eb40-4f7a-97d6-c28effb03c59", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "3" + ] + }, + "execution_count": 46, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import ProgramState\n", "ProgramState.STOPPING" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -211,22 +381,108 @@ }, { "cell_type": "code", + "execution_count": 47, "id": "9a0bc96b-7501-4e27-add3-ba20b81d1822", "metadata": { "scrolled": true }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'address': 0, 'state': 0},\n", + " {'address': 1, 'state': 0},\n", + " {'address': 2, 'state': 0},\n", + " {'address': 3, 'state': 0},\n", + " {'address': 4, 'state': 0},\n", + " {'address': 5, 'state': 0},\n", + " {'address': 6, 'state': 0},\n", + " {'address': 7, 'state': 0},\n", + " {'address': 8, 'state': 0},\n", + " {'address': 9, 'state': 0},\n", + " {'address': 10, 'state': 0},\n", + " {'address': 11, 'state': 0},\n", + " {'address': 12, 'state': 0},\n", + " {'address': 13, 'state': 0},\n", + " {'address': 14, 'state': 0},\n", + " {'address': 15, 'state': 0},\n", + " {'address': 16, 'state': 0},\n", + " {'address': 17, 'state': 0},\n", + " {'address': 18, 'state': 0},\n", + " {'address': 19, 'state': 0},\n", + " {'address': 20, 'state': 2},\n", + " {'address': 21, 'state': 2},\n", + " {'address': 22, 'state': 2},\n", + " {'address': 23, 'state': 2},\n", + " {'address': 24, 'state': 2},\n", + " {'address': 25, 'state': 2},\n", + " {'address': 26, 'state': 2},\n", + " {'address': 27, 'state': 2},\n", + " {'address': 28, 'state': 2},\n", + " {'address': 29, 'state': 2},\n", + " {'address': 30, 'state': 2},\n", + " {'address': 31, 'state': 2}]}" + ] + }, + "execution_count": 47, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_di()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 48, "id": "5b0e7de3-a9f7-4129-9ff3-20f168b560e6", "metadata": { "scrolled": true }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'state': 2, 'address': 0},\n", + " {'address': 1, 'state': 0},\n", + " {'address': 2, 'state': 2},\n", + " {'address': 3, 'state': 2},\n", + " {'address': 4, 'state': 2},\n", + " {'address': 5, 'state': 1},\n", + " {'address': 6, 'state': 2},\n", + " {'address': 7, 'state': 2},\n", + " {'address': 8, 'state': 2},\n", + " {'address': 9, 'state': 1},\n", + " {'address': 10, 'state': 2},\n", + " {'address': 11, 'state': 2},\n", + " {'address': 12, 'state': 2},\n", + " {'address': 13, 'state': 2},\n", + " {'address': 14, 'state': 2},\n", + " {'address': 15, 'state': 2},\n", + " {'address': 16, 'state': 2},\n", + " {'address': 17, 'state': 2},\n", + " {'address': 18, 'state': 2},\n", + " {'address': 19, 'state': 2},\n", + " {'address': 20, 'state': 2},\n", + " {'address': 21, 'state': 2},\n", + " {'address': 22, 'state': 2},\n", + " {'address': 23, 'state': 2},\n", + " {'address': 24, 'state': 2},\n", + " {'address': 25, 'state': 2},\n", + " {'address': 26, 'state': 2},\n", + " {'address': 27, 'state': 2},\n", + " {'address': 28, 'state': 2},\n", + " {'address': 29, 'state': 2},\n", + " {'address': 30, 'state': 2},\n", + " {'address': 31, 'state': 2}]}" + ] + }, + "execution_count": 48, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import DigitalState\n", "\n", @@ -234,151 +490,342 @@ "indy.set_do(signal)\n", "\n", "indy.get_do()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 49, "id": "fa5351ce-c08f-4cce-9bb4-4c4106d12706", "metadata": { "scrolled": true }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'state': 2, 'address': 0},\n", + " {'address': 1, 'state': 0},\n", + " {'address': 2, 'state': 2},\n", + " {'address': 3, 'state': 2},\n", + " {'address': 4, 'state': 2},\n", + " {'address': 5, 'state': 1},\n", + " {'address': 6, 'state': 2},\n", + " {'address': 7, 'state': 2},\n", + " {'address': 8, 'state': 2},\n", + " {'address': 9, 'state': 1},\n", + " {'address': 10, 'state': 2},\n", + " {'address': 11, 'state': 2},\n", + " {'address': 12, 'state': 2},\n", + " {'address': 13, 'state': 2},\n", + " {'address': 14, 'state': 2},\n", + " {'address': 15, 'state': 2},\n", + " {'address': 16, 'state': 2},\n", + " {'address': 17, 'state': 2},\n", + " {'address': 18, 'state': 2},\n", + " {'address': 19, 'state': 2},\n", + " {'address': 20, 'state': 2},\n", + " {'address': 21, 'state': 2},\n", + " {'address': 22, 'state': 2},\n", + " {'address': 23, 'state': 2},\n", + " {'address': 24, 'state': 2},\n", + " {'address': 25, 'state': 2},\n", + " {'address': 26, 'state': 2},\n", + " {'address': 27, 'state': 2},\n", + " {'address': 28, 'state': 2},\n", + " {'address': 29, 'state': 2},\n", + " {'address': 30, 'state': 2},\n", + " {'address': 31, 'state': 2}]}" + ] + }, + "execution_count": 49, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "signal = [{'address': 1, 'state': DigitalState.OFF}, \n", " {'address': 5, 'state': DigitalState.ON}, \n", " {'address': 9, 'state': DigitalState.ON}]\n", "indy.set_do(signal)\n", "indy.get_do()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 50, "id": "eea5f839-6afd-4255-b73b-e3a424a2c39b", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'voltage': 2, 'address': 0},\n", + " {'address': 1, 'voltage': 29},\n", + " {'address': 2, 'voltage': 0},\n", + " {'address': 3, 'voltage': 0},\n", + " {'address': 4, 'voltage': 0},\n", + " {'address': 5, 'voltage': 0},\n", + " {'address': 6, 'voltage': 0},\n", + " {'address': 7, 'voltage': 0}]}" + ] + }, + "execution_count": 50, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_ai()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 51, "id": "ad89ddfc-2ed2-44c2-a57b-c75cbfad2e20", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'voltage': 7, 'address': 0},\n", + " {'address': 1, 'voltage': 2},\n", + " {'address': 2, 'voltage': 0},\n", + " {'address': 3, 'voltage': 0},\n", + " {'address': 4, 'voltage': 0},\n", + " {'address': 5, 'voltage': 0},\n", + " {'address': 6, 'voltage': 0},\n", + " {'address': 7, 'voltage': 0}]}" + ] + }, + "execution_count": 51, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_ao()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 52, "id": "681567eb-fd4c-44a5-82f6-fba50a8414e1", "metadata": { "scrolled": true }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'voltage': 7, 'address': 0},\n", + " {'address': 1, 'voltage': 2},\n", + " {'address': 2, 'voltage': 0},\n", + " {'address': 3, 'voltage': 0},\n", + " {'address': 4, 'voltage': 0},\n", + " {'address': 5, 'voltage': 0},\n", + " {'address': 6, 'voltage': 0},\n", + " {'address': 7, 'voltage': 0}]}" + ] + }, + "execution_count": 52, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "signal = [{'address': 0, 'voltage': 7}, \n", " {'address': 1, 'voltage': 2}]\n", "indy.set_ao(signal)\n", "\n", "indy.get_ao()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 53, "id": "1cbf3deb-a276-47b3-9e9b-35257d0e2975", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'port': 'C', 'states': [0]}]}" + ] + }, + "execution_count": 53, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_endtool_di()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 54, "id": "31bedcc2-16f2-4d12-bf0d-db39504ba87b", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "-2" + ] + }, + "execution_count": 54, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ + "from neuromeka import EndtoolState\n", + "\n", "EndtoolState.LOW_PNP" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 55, "id": "31923ca7-3aa2-481a-ae15-00310e1d795e", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'port': 'C', 'states': [2]}]}" + ] + }, + "execution_count": 55, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# RevC board, DO 1 입력\n", "signal = [{'port': 'C', 'states': [EndtoolState.HIGH_PNP]}]\n", "indy.set_endtool_do(signal)\n", "\n", "indy.get_endtool_do()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 56, "id": "2142933a-0f4d-4e60-b7c2-70355bea247c", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'port': 'C', 'states': [1]}]}" + ] + }, + "execution_count": 56, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ - "from neuromeka import EndtoolState\n", - "\n", "signal = [{'port': 'A', 'states': [EndtoolState.HIGH_PNP, EndtoolState.HIGH_PNP]},\n", " {'port': 'B', 'states': [EndtoolState.HIGH_NPN, EndtoolState.HIGH_NPN]}]\n", "indy.set_endtool_do(signal)\n", "\n", "indy.get_endtool_do()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 57, "id": "d0e425c8-1a79-48f5-b349-49e04510993b", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'signals': [{'address': 0, 'voltage': 0},\n", + " {'address': 1, 'voltage': 0},\n", + " {'address': 2, 'voltage': 0},\n", + " {'address': 3, 'voltage': 0},\n", + " {'address': 4, 'voltage': 0},\n", + " {'address': 5, 'voltage': 0},\n", + " {'address': 6, 'voltage': 0},\n", + " {'address': 7, 'voltage': 0}]}" + ] + }, + "execution_count": 57, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_endtool_ao()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 58, "id": "0d08357c-7c99-462b-995b-91dd4e80dee7", "metadata": {}, + "outputs": [], "source": [ "# indy.set_endtool_ai()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 59, "id": "bbdb00b1-696b-4b2c-b9f5-44be1e310e40", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'num_joints': 6,\n", + " 'robot_serial': 'D11911I07006',\n", + " 'payload': 7.0,\n", + " 'io_board_fw_ver': '-1.0B',\n", + " 'core_board_fw_vers': ['-1.0C', '-1.0C', '-1.0C', '-1.0C', '-1.0C', '-1.0C'],\n", + " 'endtool_board_fw_ver': '-1.0C',\n", + " 'controller_ver': '3.2.0',\n", + " 'controller_detail': 'hotfix6',\n", + " 'controller_date': '2024.06.24'}" + ] + }, + "execution_count": 59, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_device_info()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 60, "id": "2e9ecf6d-1837-4f5b-9364-ee2f58e00e20", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'ft_Fx': 0.0,\n", + " 'ft_Fy': 0.0,\n", + " 'ft_Fz': 0.0,\n", + " 'ft_Tx': 0.0,\n", + " 'ft_Ty': 0.0,\n", + " 'ft_Tz': 0.0}" + ] + }, + "execution_count": 60, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_ft_sensor_data()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -397,15 +844,26 @@ }, { "cell_type": "code", + "execution_count": 61, "id": "7c6bce76-3c69-4e5c-b830-e39665df893c", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'StopMotion Success', 'code': '0'}" + ] + }, + "execution_count": 61, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import StopCategory\n", "\n", "indy.stop_motion(StopCategory.CAT2)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -417,13 +875,13 @@ }, { "cell_type": "code", + "execution_count": 62, "id": "57faa566-4254-46be-8432-e96fef00787a", "metadata": {}, + "outputs": [], "source": [ "indy.move_home()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -435,65 +893,111 @@ }, { "cell_type": "code", + "execution_count": 63, "id": "3743f6d0-a985-4c55-b942-a0fd3bce97b4", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'code': '0', 'msg': ''}" + ] + }, + "execution_count": 63, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "target_pos = [0, 0, -90, 0, -90, 0]\n", "indy.movej(jtarget=target_pos)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 64, "id": "0397a068-70c3-480d-a9f1-352df8fe4ec0", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'code': '0', 'msg': ''}" + ] + }, + "execution_count": 64, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "target_pos = [50, 0, -90, 0, -90, 0]\n", "indy.movej(jtarget=target_pos, vel_ratio=100, acc_ratio=300)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 65, "id": "c36555b2-d603-44ee-88fd-c3883ab252e4", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'code': '0', 'msg': ''}" + ] + }, + "execution_count": 65, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import JointBaseType\n", "\n", "target_pos = [50, 0, -90, 0, -90, 0]\n", "indy.movej(jtarget=target_pos, base_type=JointBaseType.ABSOLUTE)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 66, "id": "c7a83c77-358b-47b9-bb92-65dfc2153509", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'code': '0', 'msg': ''}" + ] + }, + "execution_count": 66, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "target_pos = [50, 0, 0, 0, 0, 0]\n", "indy.movej(jtarget=target_pos, base_type=JointBaseType.RELATIVE)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 69, "id": "0944d529-85cf-4e09-b675-9ec9fa308db1", "metadata": { "scrolled": true }, + "outputs": [], "source": [ + "import time\n", + "\n", "target_pos = [100, 0, -90, 0, -90, 0]\n", "\n", "for i in range(0,20):\n", " time.sleep(0.1)\n", " indy.movej(jtarget=target_pos, vel_ratio=50, acc_ratio=100, teaching_mode=True)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -505,14 +1009,25 @@ }, { "cell_type": "code", + "execution_count": 70, "id": "be2368aa-1d75-44eb-bde1-dea7501edcd4", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveJT Success', 'code': '0'}" + ] + }, + "execution_count": 70, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "target_pos = [-100, 0, 90, 0, 90, 0]\n", "indy.movej_time(jtarget=target_pos, move_time=5)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -524,39 +1039,72 @@ }, { "cell_type": "code", + "execution_count": 71, "id": "48959641-3825-402a-a8b1-f4ba45af3c0f", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveL Success', 'code': '0'}" + ] + }, + "execution_count": 71, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "pos = indy.get_control_state()['p']\n", "\n", "indy.movel(ttarget=pos)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 72, "id": "69128d47-1144-4c38-910a-142ddb512308", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveL Success', 'code': '0'}" + ] + }, + "execution_count": 72, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import TaskBaseType\n", "\n", "pos = [100, 0, 0, 0, 0, 0]\n", "indy.movel(ttarget=pos, base_type=TaskBaseType.RELATIVE)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 73, "id": "b73d9599-4e91-4527-ac3a-a89f0cbfefad", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveL Success', 'code': '0'}" + ] + }, + "execution_count": 73, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "pos = [100, 0, 0, 0, 0, 0]\n", "indy.movel(ttarget=pos, base_type=TaskBaseType.TCP)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -568,16 +1116,27 @@ }, { "cell_type": "code", + "execution_count": 74, "id": "7b8e468d-a8a7-4f69-bf27-5c5fbad8a23d", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveLT Success', 'code': '0'}" + ] + }, + "execution_count": 74, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "pos = indy.get_control_state()['p']\n", "pos[0] += 100\n", "\n", "indy.movel_time(ttarget=pos, move_time=3)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -589,8 +1148,21 @@ }, { "cell_type": "code", + "execution_count": 75, "id": "6c8fd7ae-2f42-48fc-aacc-7adce6c52e64", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveC Success', 'code': '0'}" + ] + }, + "execution_count": 75, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "start_pos = [400.83, -61.27, 647.45, 0.07, 179.96, 8.78]\n", "indy.movel(ttarget=start_pos)\n", @@ -598,9 +1170,7 @@ "via_point = [241.66, -51.11, 644.20, 0.0, 180.0, 23.36]\n", "end_point = [401.53, -47.74, 647.50, 0.0, 180.0, 23.37]\n", "indy.movec(tpos0=via_point, tpos1=end_point, angle=720)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -612,8 +1182,21 @@ }, { "cell_type": "code", + "execution_count": 77, "id": "c0a8b27a-4896-4f14-851a-34d7eb598047", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveL Success', 'code': '0'}" + ] + }, + "execution_count": 77, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "from neuromeka import BlendingType\n", "import time\n", @@ -629,14 +1212,25 @@ "\n", "indy.movel(target_pos2)\n", "indy.movel(target_pos3)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 78, "id": "e9398b74-d4fa-4aa8-847e-634d87bc0e3d", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveL Success', 'code': '0'}" + ] + }, + "execution_count": 78, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.movel(target_pos1)\n", "time.sleep(1)\n", @@ -644,14 +1238,25 @@ "indy.movel(target_pos2, blending_type=BlendingType.DUPLICATE)\n", "time.sleep(0.5)\n", "indy.movel(target_pos3)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 79, "id": "460c167d-a4dd-4b43-ab25-9f3c7f63aee4", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'MoveL Success', 'code': '0'}" + ] + }, + "execution_count": 79, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.movel(target_pos1)\n", "time.sleep(1)\n", @@ -659,9 +1264,7 @@ "indy.movel(target_pos2, blending_type=BlendingType.OVERRIDE)\n", "time.sleep(0.5)\n", "indy.movel(target_pos3)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -673,76 +1276,131 @@ }, { "cell_type": "code", + "execution_count": 80, "id": "df235073-5c5f-478a-97c1-988b1099a1c6", "metadata": {}, + "outputs": [], "source": [ "from neuromeka import JointTeleopType" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 81, "id": "c46292bf-ea2a-4304-892c-5d50e7dd511d", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'TeleOp Enabled', 'code': '0'}" + ] + }, + "execution_count": 81, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.start_teleop(method=JointTeleopType.ABSOLUTE)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 82, "id": "7493f6f9-bf15-4444-9093-f3aa3f2e6377", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'TeleOp Stop Requested', 'code': '0'}" + ] + }, + "execution_count": 82, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.stop_teleop()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 88, "id": "d37cca59-bf57-4610-abf4-a84c06ba3713", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'TeleOp Enabled', 'code': '0'}" + ] + }, + "execution_count": 88, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.start_teleop(method=JointTeleopType.RELATIVE)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 89, "id": "8709f24c-a5f7-4908-b61d-61f09d93ca1d", "metadata": {}, + "outputs": [], "source": [ "x_inc = 5" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 90, "id": "1764e9b0-c6ea-4fb7-af58-276cb3807893", "metadata": { "scrolled": true }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'code': '0', 'msg': ''}" + ] + }, + "execution_count": 90, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "x_inc += 5\n", "indy.movetelej_rel(jpos=[x_inc, 0, 0, 0, 0, 0], vel_ratio=1.0, acc_ratio=1.0)\n" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 91, "id": "9810e996-4790-4954-b1b3-4eafc8b4505c", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'TeleOp Stop Requested', 'code': '0'}" + ] + }, + "execution_count": 91, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.stop_teleop()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -754,18 +1412,56 @@ }, { "cell_type": "code", + "execution_count": 92, "id": "3a6aa80f-aa77-4869-a9ad-4cb17a8a3d88", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[304.12582, 2.8539128, 550.0006, 6.11203e-05, 179.99995, 32.999863]" + ] + }, + "execution_count": 92, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.get_control_data()['p']" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 93, "id": "42382ef8-ded1-4eb6-bbf7-e7a1892a151f", "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Current tpos [304.12582, 2.8539128, 550.0006, 6.1120074e-05, 179.99995, 32.999863]\n", + "Current jpos [38.35939, 13.800658, -97.074875, -6.567765e-05, -96.725746, 5.3595185]\n" + ] + }, + { + "data": { + "text/plain": [ + "{'jpos': [38.35939,\n", + " 13.800658,\n", + " -97.074875,\n", + " -6.567765e-05,\n", + " -96.725746,\n", + " 5.3595185],\n", + " 'response': {'msg': 'InverseKinematics Success', 'code': '0'}}" + ] + }, + "execution_count": 93, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "tpos = indy.get_control_data()['p']\n", "init_jpos = indy.get_control_data()['q']\n", @@ -774,27 +1470,28 @@ "print(\"Current jpos\", init_jpos)\n", "\n", "indy.inverse_kin(tpos, init_jpos)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 94, "id": "168e8b6f-5e48-40f4-9012-609b0b7d67ba", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'msg': 'SetDirectTeaching Success', 'code': '0'}" + ] + }, + "execution_count": 94, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.set_direct_teaching(False)" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "324702a8-b75e-4833-beb5-8ea2d0be6002", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -806,36 +1503,65 @@ }, { "cell_type": "code", + "execution_count": 95, "id": "60b5cfb9-1a45-4e8c-99e4-62834146d109", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'relative_pos': [132.99998, 0.0, 0.0, 25.69598, 13.915254, 3.7340522],\n", + " 'response': {'msg': 'CalculateRelativePose Success', 'code': '0'}}" + ] + }, + "execution_count": 95, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "tpos1 = [200.41444, 368.4608, 616.946, 150.90015, -1.0546277, 30.016459]\n", "tpos2 = [333.41444, 368.4608, 616.946, 179.90015, -1.0546277, 30.016459]\n", "\n", "indy.calculate_relative_pose(start_pos=tpos1, end_pos=tpos2)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 96, "id": "e62d803f-bc1b-44af-ae4c-fdf8433fb555", "metadata": {}, + "outputs": [], "source": [ "tpos3 = [132.99998, 0.0, 0.0, 25.69598, 13.915254, 3.7340522]" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 97, "id": "0f6f6f4a-7d12-4329-8220-838402e5752a", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'calculated_pos': [333.41443,\n", + " 368.4608,\n", + " 616.946,\n", + " 179.90015,\n", + " -1.054627,\n", + " 30.016459],\n", + " 'response': {'msg': 'CalculateCurrentPoseRel Success', 'code': '0'}}" + ] + }, + "execution_count": 97, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "indy.calculate_current_pose_rel(current_pos=tpos1, relative_pos=tpos3)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -847,152 +1573,238 @@ }, { "cell_type": "code", + "execution_count": 98, "id": "3880d2e3-f340-44ac-b67e-d07238376534", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'variables': []}" + ] + }, + "execution_count": 98, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# 전체 bool 변수 값 획득\n", "val = indy.get_bool_variable()\n", "val" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 99, "id": "8c93382e-2e63-4bba-96d6-579563aedf1b", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{}" + ] + }, + "execution_count": 99, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# bool 변수 주소 100에 값 True 할당\n", "indy.set_bool_variable([{'addr': 100, 'value': True}])\n", "# bool 변수 주소 150에 값 False 할당\n", "indy.set_bool_variable([{'addr': 150, 'value': False}])" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 100, "id": "4b61af29-4814-4b16-8598-acd5ffe0a86d", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'variables': []}" + ] + }, + "execution_count": 100, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# 전체 int 변수 값 획득\n", "val = indy.get_int_variable()\n", "val" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 101, "id": "12d52143-393d-4fc1-817d-6a0d4e762df3", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{}" + ] + }, + "execution_count": 101, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# int 변수 주소 10에 값 20 할당\n", "indy.set_int_variable([{'addr': 10, 'value': 20}])\n", "# int 변수 주소 15에 값 5 할당\n", "indy.set_int_variable([{'addr': 15, 'value': 5}])" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 102, "id": "ff62a41d-5f49-455b-80d9-2175d61bd4cb", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'variables': []}" + ] + }, + "execution_count": 102, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# 전체 float 변수 값 획득\n", "val = indy.get_float_variable()\n", "val" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 103, "id": "d13bb625-7d60-4dc6-98ac-bd797ea194f7", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{}" + ] + }, + "execution_count": 103, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# float 변수 주소 10에 값 -23.3 할당\n", "indy.set_float_variable([{'addr': 10, 'value': -23.3}])\n", "# float 변수 주소 15에 값 55.12 할당\n", "indy.set_float_variable([{'addr': 15, 'value': 55.12}])" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "52aed4c1-ef90-4230-a639-d219073a9248", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 104, "id": "3a7de6fe-9713-403c-9d9f-9f66584ac88c", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[]" + ] + }, + "execution_count": 104, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# 전체 jpos 변수 값 획득\n", "val = indy.get_jpos_variable()\n", "val" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 105, "id": "f66cd946-a97d-4f65-9495-a07b7876f1f0", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{}" + ] + }, + "execution_count": 105, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# jpos 변수 주소 10에 값 [0,0,0,0,0,0] 할당\n", "indy.set_jpos_variable([{'addr': 0, 'jpos': [0,0,0,0,0,0]}])\n", "# jpos 변수 주소 15에 값 [10,10,10,10,10,10] 할당\n", "indy.set_jpos_variable([{'addr': 200, 'jpos': [10,10,10,10,10,10]}])" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 106, "id": "a05a19d2-b82c-494e-b2d8-ee1d33ab1a06", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'variables': []}" + ] + }, + "execution_count": 106, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# 전체 tpos 변수 값 획득\n", "val = indy.get_tpos_variable()\n", "val" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 107, "id": "f6630b0f-14c5-47f7-9dc1-72828fe94535", "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{}" + ] + }, + "execution_count": 107, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# tpos 변수 주소 10에 값 [100,100,200,0,180,0] 할당\n", "indy.set_tpos_variable([{'addr': 10, 'tpos': [100,100,200,0,180,0]}])\n", "# jpos 변수 주소 20에 값 [100,100,350,0,180,0] 할당\n", "indy.set_tpos_variable([{'addr': 20, 'tpos': [100,100,350,0,180,0]}])" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "64f4e2ff-6947-4506-bab7-c772e4c012e4", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "9135c1b8-90cc-4d52-9c4c-067dec166bba", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -1004,81 +1816,73 @@ }, { "cell_type": "code", + "execution_count": null, "id": "9d0efa3b-ec2f-46da-873d-1be5cd941442", "metadata": {}, + "outputs": [], "source": [ "indy.play_program(prog_idx=3)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "59577a8e-c938-4851-b381-049aed454182", "metadata": {}, + "outputs": [], "source": [ "indy.play_program(prog_name='t555.indy7v2.json')" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "140271f7-817d-4642-bbbb-7ce817ba0f11", "metadata": {}, + "outputs": [], "source": [ "indy.pause_program()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "d0e6c16a-a209-4bac-bec6-65bc97dc238a", "metadata": {}, + "outputs": [], "source": [ "indy.resume_program()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "ac38dddb-cccc-4351-8443-d049c7eb8051", "metadata": {}, + "outputs": [], "source": [ "indy.stop_program()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "4c219aef-fe9a-4540-92a3-355a5fb3f2f0", "metadata": {}, + "outputs": [], "source": [ "indy.set_speed_ratio(10)" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "e31dab8e-9dca-4d48-85ff-87b1701bbab7", "metadata": {}, + "outputs": [], "source": [ "indy.set_speed_ratio(100)" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "a47b7903-81cf-4a1c-baef-c91d64c57466", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -1090,149 +1894,133 @@ }, { "cell_type": "code", + "execution_count": null, "id": "cf6e0561-cce4-47a0-9408-c9b6dcd4cee2", "metadata": {}, + "outputs": [], "source": [ "indy.get_home_pos()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "d36d05f5-5c99-4790-841e-d92196d9225c", "metadata": {}, + "outputs": [], "source": [ "indy.set_home_pos([-150, 5.0, 90.0, 0.0, 85.0, 0.0])" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "43aeef8e-b771-493f-b700-bebef23c8f6c", "metadata": {}, + "outputs": [], "source": [ "indy.get_ref_frame()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "8488474e-ba68-45f1-a158-93a6a080a396", "metadata": {}, + "outputs": [], "source": [ "indy.set_ref_frame([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "4a35908f-efe0-4655-9bca-a9c5def3d521", "metadata": {}, - "source": [ - "indy.get_ref_frame()" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "d10a068c-156c-4381-87a1-39120387a079", - "metadata": {}, - "source": [], "outputs": [], - "execution_count": null + "source": [ + "indy.get_ref_frame()" + ] }, { "cell_type": "code", + "execution_count": null, "id": "1def6b5d-0542-4fe6-ab12-cf89e34d9ad1", "metadata": {}, + "outputs": [], "source": [ "indy.get_friction_comp()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "5a0025f9-cac5-45a0-ba24-9b3c341addbd", "metadata": {}, + "outputs": [], "source": [ "indy.get_tool_property()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "4530f422-9146-438a-9738-ad8c97a7d6d3", "metadata": {}, + "outputs": [], "source": [ "indy.get_coll_sens_level()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "b8a0d4e6-e3a9-4e81-be08-1fe9a4784040", "metadata": {}, + "outputs": [], "source": [ "indy.get_coll_sens_param()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "ece6e0a9-f0a5-4332-9227-eeb4db73aacc", "metadata": {}, + "outputs": [], "source": [ "indy.get_coll_policy()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "4316f92d-ad20-4bf9-87ec-94a3f820ddc7", "metadata": {}, + "outputs": [], "source": [ "indy.get_safety_limits()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "77c11cbf-5ffc-44a6-92aa-40c11c2735c7", "metadata": {}, + "outputs": [], "source": [ "indy.set_safety_limits()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "7851c565-f611-470a-bd0b-20028b61c44d", "metadata": {}, + "outputs": [], "source": [ "indy.get_coll_policy()" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "df6baeb4-16b6-4a83-99f7-1cde95f8a915", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -1244,31 +2032,23 @@ }, { "cell_type": "code", + "execution_count": null, "id": "e43f75d1-2e0c-4d23-99a3-040b776e5cf1", "metadata": {}, + "outputs": [], "source": [ "indy.activate_sdk(license_key=\"\", expire_date=\"2024-12-20\")" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": null, "id": "e5b59d1d-9d84-4ca0-8b47-5c067cc08672", "metadata": {}, + "outputs": [], "source": [ "indy.get_custom_control_mode()" - ], - "outputs": [], - "execution_count": null - }, - { - "cell_type": "code", - "id": "cf751cdb-9f01-40c2-9d80-77d800da3880", - "metadata": {}, - "source": [], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -1280,23 +2060,23 @@ }, { "cell_type": "code", + "execution_count": 109, "id": "f5987246-4186-46c2-ad3d-351c998acb84", "metadata": {}, + "outputs": [], "source": [ "indy.start_log()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "code", + "execution_count": 110, "id": "36047a98-0a36-4237-bc40-949e36532d71", "metadata": {}, + "outputs": [], "source": [ "indy.end_log()" - ], - "outputs": [], - "execution_count": null + ] }, { "cell_type": "markdown", @@ -1316,6 +2096,7 @@ }, { "cell_type": "code", + "execution_count": null, "id": "0d8aea84-591b-4005-85d1-982dc2dbc67a", "metadata": { "ExecuteTime": { @@ -1323,6 +2104,18 @@ "start_time": "2024-07-10T16:33:54.381147Z" } }, + "outputs": [ + { + "data": { + "text/plain": [ + "{'code': '0', 'msg': ''}" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "# gain0 = Kp for Joint\n", "# gain1 = Kv for Joint\n", @@ -1340,23 +2133,11 @@ "gain5 = [800, 800, 600, 400, 400, 400]\n", "\n", "indy.set_custom_control_gain6(gain0=gain0, gain1=gain1, gain2=gain2, gain3=gain3, gain4=gain4, gain5=gain5)" - ], - "outputs": [ - { - "data": { - "text/plain": [ - "{'code': '0', 'msg': ''}" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "execution_count": 4 + ] }, { "cell_type": "code", + "execution_count": null, "id": "ca2106f9-c422-4779-85c2-595f0f606d8d", "metadata": { "ExecuteTime": { @@ -1364,9 +2145,6 @@ "start_time": "2024-07-10T16:34:04.499593Z" } }, - "source": [ - "indy.get_custom_control_gain()" - ], "outputs": [ { "data": { @@ -1388,7 +2166,9 @@ "output_type": "execute_result" } ], - "execution_count": 5 + "source": [ + "indy.get_custom_control_gain()" + ] }, { "cell_type": "markdown", @@ -1400,6 +2180,7 @@ }, { "cell_type": "code", + "execution_count": null, "id": "8e96e899-c223-4dd1-96ae-37ad85dc0afb", "metadata": { "ExecuteTime": { @@ -1407,10 +2188,6 @@ "start_time": "2024-07-10T16:34:11.424431Z" } }, - "source": [ - "mode = 1\n", - "indy.set_custom_control_mode(mode=mode)" - ], "outputs": [ { "data": { @@ -1423,10 +2200,14 @@ "output_type": "execute_result" } ], - "execution_count": 6 + "source": [ + "mode = 1\n", + "indy.set_custom_control_mode(mode=mode)" + ] }, { "cell_type": "code", + "execution_count": null, "id": "7dadc517-206b-4756-beb5-581071ca5397", "metadata": { "ExecuteTime": { @@ -1434,9 +2215,6 @@ "start_time": "2024-07-10T16:34:14.025288Z" } }, - "source": [ - "indy.get_custom_control_mode()" - ], "outputs": [ { "data": { @@ -1449,7 +2227,9 @@ "output_type": "execute_result" } ], - "execution_count": 7 + "source": [ + "indy.get_custom_control_mode()" + ] }, { "cell_type": "markdown", @@ -1461,6 +2241,7 @@ }, { "cell_type": "code", + "execution_count": null, "id": "02bd2528-6694-4ab7-a69b-db8dd34738c8", "metadata": { "ExecuteTime": { @@ -1468,6 +2249,7 @@ "start_time": "2024-07-10T16:34:17.479828Z" } }, + "outputs": [], "source": [ "def joint_move(jtarget):\n", " flag = 0\n", @@ -1482,12 +2264,11 @@ " if indy.get_motion_data()['is_target_reached']:\n", " flag = 0\n", " break" - ], - "outputs": [], - "execution_count": 8 + ] }, { "cell_type": "code", + "execution_count": null, "id": "3c1a8c45-a3f9-4dc8-bdfe-a2ffeb35abcb", "metadata": { "ExecuteTime": { @@ -1495,15 +2276,6 @@ "start_time": "2024-07-10T16:34:41.603875Z" } }, - "source": [ - "print(\"start\")\n", - "joint_move(jtarget = [0,0,0,0,0,0])\n", - "joint_move(jtarget = [-44,25,-63,48,-7,-105])\n", - "joint_move(jtarget = [0,0,90,0,90,0])\n", - "joint_move(jtarget = [-145,31,-33,117,-7,-133])\n", - "joint_move(jtarget = [-90,-15,-90,0,-75,0])\n", - "print(\"finish\")" - ], "outputs": [ { "name": "stdout", @@ -1514,7 +2286,15 @@ ] } ], - "execution_count": 9 + "source": [ + "print(\"start\")\n", + "joint_move(jtarget = [0,0,0,0,0,0])\n", + "joint_move(jtarget = [-44,25,-63,48,-7,-105])\n", + "joint_move(jtarget = [0,0,90,0,90,0])\n", + "joint_move(jtarget = [-145,31,-33,117,-7,-133])\n", + "joint_move(jtarget = [-90,-15,-90,0,-75,0])\n", + "print(\"finish\")" + ] }, { "cell_type": "markdown", @@ -1526,6 +2306,7 @@ }, { "cell_type": "code", + "execution_count": null, "id": "f9d2b013-67b7-4f8d-908d-ae0879d3957d", "metadata": { "ExecuteTime": { @@ -1533,17 +2314,6 @@ "start_time": "2024-07-10T16:35:27.410395Z" } }, - "source": [ - "# gain2 value is changed from default\n", - "gain0 = [100, 100, 100,100, 100, 100]\n", - "gain1 = [20, 20, 20, 20, 20, 20]\n", - "gain2 = [900, 900, 500, 300, 300, 300]\n", - "gain3 = [100, 100, 100,100, 100, 100]\n", - "gain4 = [20, 20, 20, 20, 20, 20]\n", - "gain5 = [800, 800, 600, 400, 400, 400]\n", - "\n", - "indy.set_custom_control_gain6(gain0=gain0, gain1=gain1, gain2=gain2, gain3=gain3, gain4=gain4, gain5=gain5)" - ], "outputs": [ { "data": { @@ -1556,10 +2326,21 @@ "output_type": "execute_result" } ], - "execution_count": 10 + "source": [ + "# gain2 value is changed from default\n", + "gain0 = [100, 100, 100,100, 100, 100]\n", + "gain1 = [20, 20, 20, 20, 20, 20]\n", + "gain2 = [900, 900, 500, 300, 300, 300]\n", + "gain3 = [100, 100, 100,100, 100, 100]\n", + "gain4 = [20, 20, 20, 20, 20, 20]\n", + "gain5 = [800, 800, 600, 400, 400, 400]\n", + "\n", + "indy.set_custom_control_gain6(gain0=gain0, gain1=gain1, gain2=gain2, gain3=gain3, gain4=gain4, gain5=gain5)" + ] }, { "cell_type": "code", + "execution_count": null, "id": "48f3b801-8a44-4556-a3a9-1f35568e3ce8", "metadata": { "ExecuteTime": { @@ -1567,9 +2348,6 @@ "start_time": "2024-07-10T16:35:30.819084Z" } }, - "source": [ - "indy.get_custom_control_gain()" - ], "outputs": [ { "data": { @@ -1591,10 +2369,13 @@ "output_type": "execute_result" } ], - "execution_count": 11 + "source": [ + "indy.get_custom_control_gain()" + ] }, { "cell_type": "code", + "execution_count": null, "id": "4809844c-eff8-4f0c-bd5a-621a0c97ab93", "metadata": { "ExecuteTime": { @@ -1602,15 +2383,6 @@ "start_time": "2024-07-10T16:35:35.876112Z" } }, - "source": [ - "print(\"start\")\n", - "joint_move(jtarget = [0,0,0,0,0,0])\n", - "joint_move(jtarget = [-44,25,-63,48,-7,-105])\n", - "joint_move(jtarget = [0,0,90,0,90,0])\n", - "joint_move(jtarget = [-145,31,-33,117,-7,-133])\n", - "joint_move(jtarget = [-90,-15,-90,0,-75,0])\n", - "print(\"finish\")" - ], "outputs": [ { "name": "stdout", @@ -1621,7 +2393,15 @@ ] } ], - "execution_count": 12 + "source": [ + "print(\"start\")\n", + "joint_move(jtarget = [0,0,0,0,0,0])\n", + "joint_move(jtarget = [-44,25,-63,48,-7,-105])\n", + "joint_move(jtarget = [0,0,90,0,90,0])\n", + "joint_move(jtarget = [-145,31,-33,117,-7,-133])\n", + "joint_move(jtarget = [-90,-15,-90,0,-75,0])\n", + "print(\"finish\")" + ] }, { "cell_type": "markdown", @@ -1633,6 +2413,7 @@ }, { "cell_type": "code", + "execution_count": null, "id": "abba16d5-648c-4ea0-a716-2da4449cf718", "metadata": { "ExecuteTime": { @@ -1640,6 +2421,7 @@ "start_time": "2024-07-10T16:36:01.746769Z" } }, + "outputs": [], "source": [ "def task_move(ttarget):\n", " flag = 0\n", @@ -1654,12 +2436,11 @@ " if indy.get_motion_data()['is_target_reached']:\n", " flag = 0\n", " break" - ], - "outputs": [], - "execution_count": 13 + ] }, { "cell_type": "code", + "execution_count": null, "id": "389f4c08-9834-459b-ab7b-a3b71a1efd07", "metadata": { "ExecuteTime": { @@ -1667,15 +2448,6 @@ "start_time": "2024-07-10T16:36:22.025164Z" } }, - "source": [ - "print(\"start\")\n", - "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", - "task_move(ttarget = [-334.67, -493.07, 259.00, 179.96, -0.12, 89.97])\n", - "task_move(ttarget = [224.79, -490.20, 508.08, 179.96, -0.14, 89.97])\n", - "task_move(ttarget = [-129.84, -416.84, 507.38, 179.95, -0.16, 89.96])\n", - "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", - "print(\"finish\")\n" - ], "outputs": [ { "name": "stdout", @@ -1686,10 +2458,19 @@ ] } ], - "execution_count": 14 + "source": [ + "print(\"start\")\n", + "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", + "task_move(ttarget = [-334.67, -493.07, 259.00, 179.96, -0.12, 89.97])\n", + "task_move(ttarget = [224.79, -490.20, 508.08, 179.96, -0.14, 89.97])\n", + "task_move(ttarget = [-129.84, -416.84, 507.38, 179.95, -0.16, 89.96])\n", + "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", + "print(\"finish\")\n" + ] }, { "cell_type": "code", + "execution_count": null, "id": "9980e333-29f4-44ac-9144-6ab82b1e61d1", "metadata": { "ExecuteTime": { @@ -1697,17 +2478,6 @@ "start_time": "2024-07-10T16:36:39.905055Z" } }, - "source": [ - "# gain5 value is changed from default\n", - "gain0 = [100, 100, 100,100, 100, 100]\n", - "gain1 = [20, 20, 20, 20, 20, 20]\n", - "gain2 = [800, 800, 600, 400, 400, 400]\n", - "gain3 = [100, 100, 100,100, 100, 100]\n", - "gain4 = [20, 20, 20, 20, 20, 20]\n", - "gain5 = [900, 900, 500, 300, 300, 300]\n", - "\n", - "indy.set_custom_control_gain6(gain0=gain0, gain1=gain1, gain2=gain2, gain3=gain3, gain4=gain4, gain5=gain5)" - ], "outputs": [ { "data": { @@ -1720,10 +2490,21 @@ "output_type": "execute_result" } ], - "execution_count": 15 + "source": [ + "# gain5 value is changed from default\n", + "gain0 = [100, 100, 100,100, 100, 100]\n", + "gain1 = [20, 20, 20, 20, 20, 20]\n", + "gain2 = [800, 800, 600, 400, 400, 400]\n", + "gain3 = [100, 100, 100,100, 100, 100]\n", + "gain4 = [20, 20, 20, 20, 20, 20]\n", + "gain5 = [900, 900, 500, 300, 300, 300]\n", + "\n", + "indy.set_custom_control_gain6(gain0=gain0, gain1=gain1, gain2=gain2, gain3=gain3, gain4=gain4, gain5=gain5)" + ] }, { "cell_type": "code", + "execution_count": null, "id": "e3f3b3d5-9c27-4a9d-b47a-194f873d5584", "metadata": { "ExecuteTime": { @@ -1731,9 +2512,6 @@ "start_time": "2024-07-10T16:36:42.280911Z" } }, - "source": [ - "indy.get_custom_control_gain()" - ], "outputs": [ { "data": { @@ -1755,10 +2533,13 @@ "output_type": "execute_result" } ], - "execution_count": 16 + "source": [ + "indy.get_custom_control_gain()" + ] }, { "cell_type": "code", + "execution_count": null, "id": "7c70457d-6ae2-4248-acd3-3d59c611218b", "metadata": { "ExecuteTime": { @@ -1766,15 +2547,6 @@ "start_time": "2024-07-10T16:36:49.467487Z" } }, - "source": [ - "print(\"start\")\n", - "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", - "task_move(ttarget = [-334.67, -493.07, 259.00, 179.96, -0.12, 89.97])\n", - "task_move(ttarget = [224.79, -490.20, 508.08, 179.96, -0.14, 89.97])\n", - "task_move(ttarget = [-129.84, -416.84, 507.38, 179.95, -0.16, 89.96])\n", - "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", - "print(\"finish\")\n" - ], "outputs": [ { "name": "stdout", @@ -1785,10 +2557,19 @@ ] } ], - "execution_count": 17 + "source": [ + "print(\"start\")\n", + "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", + "task_move(ttarget = [-334.67, -493.07, 259.00, 179.96, -0.12, 89.97])\n", + "task_move(ttarget = [224.79, -490.20, 508.08, 179.96, -0.14, 89.97])\n", + "task_move(ttarget = [-129.84, -416.84, 507.38, 179.95, -0.16, 89.96])\n", + "task_move(ttarget = [-186.54, -454.45, 415.61, 179.99, -0.06,89.98])\n", + "print(\"finish\")\n" + ] }, { "cell_type": "code", + "execution_count": null, "id": "8bc59d38-c177-4bef-8340-7092f881ef5f", "metadata": { "ExecuteTime": { @@ -1796,10 +2577,6 @@ "start_time": "2024-07-10T16:37:06.078767Z" } }, - "source": [ - "mode = 0\n", - "indy.set_custom_control_mode(mode=mode)" - ], "outputs": [ { "data": { @@ -1812,10 +2589,14 @@ "output_type": "execute_result" } ], - "execution_count": 18 + "source": [ + "mode = 0\n", + "indy.set_custom_control_mode(mode=mode)" + ] }, { "cell_type": "code", + "execution_count": null, "id": "04a5dc7e-0f86-4769-8927-ae089b2e5201", "metadata": { "ExecuteTime": { @@ -1823,9 +2604,6 @@ "start_time": "2024-07-10T16:37:09.022251Z" } }, - "source": [ - "indy.get_custom_control_mode()" - ], "outputs": [ { "data": { @@ -1838,7 +2616,9 @@ "output_type": "execute_result" } ], - "execution_count": 19 + "source": [ + "indy.get_custom_control_mode()" + ] } ], "metadata": { @@ -1857,7 +2637,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.0" + "version": "3.9.13" } }, "nbformat": 4, diff --git a/python/examples/indyeye_example.ipynb b/python/examples/indyeye_example.ipynb index 59cd936..e886cc2 100644 --- a/python/examples/indyeye_example.ipynb +++ b/python/examples/indyeye_example.ipynb @@ -46,22 +46,6 @@ "pose_obj = eye.detect(0, task_pos, mode=DetectKey.REF_TO_OBJECT)\n", "print(pose_obj)" ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "f8c2db3b-ac5f-4592-8113-0b4645329909", - "metadata": {}, - "outputs": [], - "source": [] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "040ad64d-b65c-4706-87e5-fde8c774e92d", - "metadata": {}, - "outputs": [], - "source": [] } ], "metadata": { diff --git a/python/neuromeka.egg-info/PKG-INFO b/python/neuromeka.egg-info/PKG-INFO index 1077d56..5f83454 100644 --- a/python/neuromeka.egg-info/PKG-INFO +++ b/python/neuromeka.egg-info/PKG-INFO @@ -1,6 +1,6 @@ Metadata-Version: 2.1 Name: neuromeka -Version: 3.2.0.5 +Version: 3.2.0.7 Summary: Neuromeka client protocols for IndyDCP3, IndyEye, Moby, Ecat, and Motor Home-page: https://github.com/neuromeka-robotics/neuromeka-package Author: Neuromeka @@ -9,33 +9,34 @@ Classifier: Development Status :: 3 - Alpha Classifier: Intended Audience :: Developers Classifier: License :: OSI Approved :: MIT License Classifier: Operating System :: OS Independent -Classifier: Programming Language :: Python :: 3 Classifier: Programming Language :: Python :: 3.6 Classifier: Programming Language :: Python :: 3.7 Classifier: Programming Language :: Python :: 3.8 Classifier: Programming Language :: Python :: 3.9 -Classifier: Programming Language :: Python :: 3.10 Requires-Python: >=3.6 Description-Content-Type: text/markdown -Requires-Dist: grpcio==1.39.0 -Requires-Dist: grpcio-tools==1.39.0 -Requires-Dist: protobuf==3.17.3 -Requires-Dist: requests==2.22.0 +Requires-Dist: grpcio<=1.44.0,>=1.39.0 +Requires-Dist: grpcio-tools<=1.44.0,>=1.39.0 +Requires-Dist: protobuf<=3.20.3,>=3.17.3 +Requires-Dist: requests<=2.32.3,>=2.22.0 Requires-Dist: Pillow==9.5.0 Requires-Dist: numpy==1.21.6 +Requires-Dist: pandas-stubs<=2.0.1.230501 Requires-Dist: pyModbusTCP==0.2.1 -Requires-Dist: netifaces==0.11.0 +Requires-Dist: netifaces2 -# Neuromeka Clients +# Neuromeka Robot API ![PyPI](https://img.shields.io/pypi/v/neuromeka) -This package provides client protocols for users to interact with Neuromeka's products, including Indy, Moby, Ecat, and Motor. +This package provides client protocols for users to interact with Neuromeka's robot products, including Indy, Moby, Ecat, and Motor. * Website: http://www.neuromeka.com * Source code: https://github.com/neuromeka-robotics/neuromeka-package * PyPI package: https://pypi.org/project/neuromeka/ * Documents: https://docs.neuromeka.com + + ## Installation ### Python diff --git a/python/neuromeka.egg-info/SOURCES.txt b/python/neuromeka.egg-info/SOURCES.txt index b34b8e1..3324be1 100644 --- a/python/neuromeka.egg-info/SOURCES.txt +++ b/python/neuromeka.egg-info/SOURCES.txt @@ -1,4 +1,3 @@ -README.md setup.py neuromeka/__init__.py neuromeka/ecat.py diff --git a/python/neuromeka.egg-info/requires.txt b/python/neuromeka.egg-info/requires.txt index dd5ef17..0bb7a9c 100644 --- a/python/neuromeka.egg-info/requires.txt +++ b/python/neuromeka.egg-info/requires.txt @@ -1,8 +1,9 @@ -grpcio==1.39.0 -grpcio-tools==1.39.0 -protobuf==3.17.3 -requests==2.22.0 +grpcio<=1.44.0,>=1.39.0 +grpcio-tools<=1.44.0,>=1.39.0 +protobuf<=3.20.3,>=3.17.3 +requests<=2.32.3,>=2.22.0 Pillow==9.5.0 numpy==1.21.6 +pandas-stubs<=2.0.1.230501 pyModbusTCP==0.2.1 -netifaces==0.11.0 +netifaces2 diff --git a/python/neuromeka/eye.py b/python/neuromeka/eye.py index 65d7b4c..6c5f6ad 100644 --- a/python/neuromeka/eye.py +++ b/python/neuromeka/eye.py @@ -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 @@ -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), @@ -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 @@ -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)) @@ -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, @@ -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, @@ -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) @@ -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 diff --git a/python/neuromeka/moby.py b/python/neuromeka/moby.py index 3785141..73a7a3e 100644 --- a/python/neuromeka/moby.py +++ b/python/neuromeka/moby.py @@ -3,9 +3,9 @@ import grpc, time - MOBY_PORT = 20200 + class MobyClient: def __init__(self, ip_addr): moby_channel = grpc.insecure_channel("{}:{}".format(ip_addr, MOBY_PORT)) @@ -32,7 +32,7 @@ def get_moby_state(self): 7: "MOVING", 16: "TELE_OP", } - state = self.__moby_stub.GetMobyState(Empty()) + state = self.__moby_stub.GetMobyState(common_msgs.Empty()) moby_state = state_dict.get(state.status, "(UNKNOWN STATE CODE)") b_ready = state.is_ready b_enable = state.is_enable @@ -58,21 +58,21 @@ def get_moby_error_state(self): 0x100: "HW_CONNECTION_LOST", } - err = self.__moby_stub.GetMobyErrorState(Empty()) + err = self.__moby_stub.GetMobyErrorState(common_msgs.Empty()) return [error_dict.get(err.errorState, "(UNKNOWN ERROR CODE)"), err.errorIndex1, err.errorIndex2, err.errorIndex3] @Utils.exception_handler def recover(self): - return self.__moby_stub.Recover(Empty()) + return self.__moby_stub.Recover(common_msgs.Empty()) @Utils.exception_handler def get_moby_pose(self): """ Get Moby pose (m): [Px, Py, Pw] """ - pose = self.__moby_stub.GetMobyPose(Empty()) + pose = self.__moby_stub.GetMobyPose(common_msgs.Empty()) return [pose.px, pose.py, pose.pw] @Utils.exception_handler @@ -80,7 +80,7 @@ def get_moby_vel(self): """ Get Moby velocity (m/s): [Vx, Vy, Vw] """ - vel = self.__moby_stub.GetMobyVel(Empty()) + vel = self.__moby_stub.GetMobyVel(common_msgs.Empty()) return [vel.vx, vel.vy, vel.vw] @Utils.exception_handler @@ -88,30 +88,30 @@ def reset_moby_pose(self): """ Reset Moby pose """ - return self.__moby_stub.ResetMobyPose(Empty()) + return self.__moby_stub.ResetMobyPose(common_msgs.Empty()) @Utils.exception_handler def get_rotation_angle(self): """ Get rotation angle (deg): [fl, fr, bl, br] """ - val = self.__moby_stub.GetRotationAngleDeg(Empty()) - return {'fl': val.fl, 'fr': val.fr, 'bl':val.bl, 'br':val.br} + val = self.__moby_stub.GetRotationAngleDeg(common_msgs.Empty()) + return {'fl': val.fl, 'fr': val.fr, 'bl': val.bl, 'br': val.br} @Utils.exception_handler def get_drive_speed(self): """ Get drive speed (m/s): [fl, fr, bl, br] """ - val = self.__moby_stub.GetDriveSpeed(Empty()) - return {'fl': val.fl, 'fr': val.fr, 'bl':val.bl, 'br':val.br} + val = self.__moby_stub.GetDriveSpeed(common_msgs.Empty()) + return {'fl': val.fl, 'fr': val.fr, 'bl': val.bl, 'br': val.br} @Utils.exception_handler def get_target_vel(self): """ Get Moby's target velocity """ - target = self.__moby_stub.GetTargetVel(Empty()) + target = self.__moby_stub.GetTargetVel(common_msgs.Empty()) return [target.vx, target.vy, target.vw] @Utils.exception_handler @@ -120,22 +120,22 @@ def get_zero(self): Get rotation's zero position (encoder count) [fl, fr, bl, br] """ - val = self.__moby_stub.GetRotationZeroCount(Empty()) - return {'fl': val.fl, 'fr': val.fr, 'bl':val.bl, 'br':val.br} + val = self.__moby_stub.GetRotationZeroCount(common_msgs.Empty()) + return {'fl': val.fl, 'fr': val.fr, 'bl': val.bl, 'br': val.br} @Utils.exception_handler def get_gyro_data(self): """ Get Gyro sensor data (yaw, yaw rate) """ - return self.__moby_stub.GetGyroData(Empty()).val + return self.__moby_stub.GetGyroData(common_msgs.Empty()).val @Utils.exception_handler def get_imu_data(self): """ Get Full IMU sensor data """ - data = self.__moby_stub.GetGyroFullData(Empty()) + data = self.__moby_stub.GetGyroFullData(common_msgs.Empty()) angle = [data.angleX, data.angleY, data.angleZ] vel = [data.angleVelX, data.angleVelY, data.angleVelZ] acc = [data.linAccX, data.linAccY, data.linAccZ] @@ -146,23 +146,29 @@ def reset_gyro(self): """ Reset gyro sensor """ - return self.__moby_stub.ResetGyroSensor(Empty()) + return self.__moby_stub.ResetGyroSensor(common_msgs.Empty()) @Utils.exception_handler def use_gyro_for_odom(self, use_gyro): """ Use gyro sensor for odometry calculation """ - return self.__moby_stub.UseGyroForOdom(BoolVal(val=use_gyro)) + return self.__moby_stub.UseGyroForOdom(moby_msgs.BoolVal(val=use_gyro)) @Utils.exception_handler def get_us_data(self): """ Get US sensor data """ - value = self.__moby_stub.GetUSSensorData(Empty()) - return {'front_left1': value.us_front_left1, 'front_left2': value.us_front_left2, 'front_left3': value.us_front_left3, 'front_ground': value.us_front_ground, 'front_right1': value.us_front_right1, 'front_right2': value.us_front_right2, 'front_right3': value.us_front_right3, 'front_right4': value.us_front_right4, 'back_right1': value.us_back_right1, 'back_right2': value.us_back_right2, 'back_right3': value.us_back_right3, 'back_ground': value.us_back_ground, 'back_left1': value.us_back_left1, 'back_left2': value.us_back_left2, 'back_left3': value.us_back_left3, 'back_left4': value.us_back_left4} - + value = self.__moby_stub.GetUSSensorData(common_msgs.Empty()) + return {'front_left1': value.us_front_left1, 'front_left2': value.us_front_left2, + 'front_left3': value.us_front_left3, 'front_ground': value.us_front_ground, + 'front_right1': value.us_front_right1, 'front_right2': value.us_front_right2, + 'front_right3': value.us_front_right3, 'front_right4': value.us_front_right4, + 'back_right1': value.us_back_right1, 'back_right2': value.us_back_right2, + 'back_right3': value.us_back_right3, 'back_ground': value.us_back_ground, + 'back_left1': value.us_back_left1, 'back_left2': value.us_back_left2, 'back_left3': value.us_back_left3, + 'back_left4': value.us_back_left4} @Utils.exception_handler def get_bms(self): @@ -184,7 +190,7 @@ def get_bms(self): 'Remain Capacity Ah', 'Remain Capacity Wh' 'Temperature-(1~3)', 'Cell Voltage-(1~13)' """ - value = self.__moby_stub.GetBMSData(Empty()) + value = self.__moby_stub.GetBMSData(common_msgs.Empty()) return {'BMS status-1': value.bms_status[0] / 10, 'BMS status-2': value.bms_status[1] / 10, 'Pack voltage-1': value.pack_volt[0] / 100, 'Pack voltage-2': value.pack_volt[1] / 100, 'Battery Voltage-1': value.battery_volt[0] / 100, 'Battery Voltage-2': value.battery_volt[1] / 100, @@ -211,35 +217,35 @@ def set_target_vel(self, vx, vy, vw): """ Drive control """ - return self.__moby_stub.SetStepControl(TargetVel(vx=vx, vy=vy, vw=vw)) + return self.__moby_stub.SetStepControl(moby_msgs.TargetVel(vx=vx, vy=vy, vw=vw)) @Utils.exception_handler def stop_motion(self): """ Stop Moby motion """ - return self.__moby_stub.StopMotion(Empty()) + return self.__moby_stub.StopMotion(common_msgs.Empty()) @Utils.exception_handler def go_straight(self): """ Go straight (zero rotation) """ - return self.__moby_stub.SetRotationAngleDeg(SwerveDoubles(fl=0, fr=0, bl=0, br=0)) + return self.__moby_stub.SetRotationAngleDeg(moby_msgs.SwerveDoubles(fl=0, fr=0, bl=0, br=0)) @Utils.exception_handler def move_rotation_deg(self, fl, fr, bl, br): """ Rotation control (target angle degree) """ - return self.__moby_stub.SetRotationAngleDeg(SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) + return self.__moby_stub.SetRotationAngleDeg(moby_msgs.SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) @Utils.exception_handler def move_driving_mps(self, fl, fr, bl, br): """ Driving control (target speed m/s) """ - return self.__moby_stub.DriveWheel(SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) + return self.__moby_stub.DriveWheel(moby_msgs.SwerveDoubles(fr=fr, br=br, bl=bl, fl=fl)) ############################ # Set Moby parameters @@ -250,14 +256,14 @@ def set_zero_as_current(self): """ Set current roation position as zero """ - return self.__moby_stub.SetZeroPosAsCurrentPos(Empty()) + return self.__moby_stub.SetZeroPosAsCurrentPos(common_msgs.Empty()) @Utils.exception_handler def set_rotation_vel_acc(self, vel, acc): """ Set rotation maximum velocity, acceleration """ - return self.__moby_stub.SetRotationVelAcc(DoubleVals(val=[vel, acc])) + return self.__moby_stub.SetRotationVelAcc(moby_msgs.DoubleVals(val=[vel, acc])) @Utils.exception_handler def set_rotation_interpolator(self, val): @@ -268,21 +274,21 @@ def set_rotation_interpolator(self, val): 2: Velocity interpolator 3: Trapezoidal interpolator """ - return self.__moby_stub.SetRotationInterpolator(IntVal(val=val)) + return self.__moby_stub.SetRotationInterpolator(moby_msgs.IntVal(val=val)) @Utils.exception_handler def set_drive_acc_dec(self, acc, dec): """ Set drive acc dec """ - return self.__moby_stub.SetDriveAccDec(DoubleVals(val=[acc, dec])) + return self.__moby_stub.SetDriveAccDec(moby_msgs.DoubleVals(val=[acc, dec])) @Utils.exception_handler def set_drive_interpolator_on_off(self, on): """ Set drive interpolator On Off """ - return self.__moby_stub.SetDriveInterpolatorOnOff(BoolVal(val=on)) + return self.__moby_stub.SetDriveInterpolatorOnOff(moby_msgs.BoolVal(val=on)) @Utils.exception_handler def set_rotation_controller_type(self, val): @@ -292,69 +298,68 @@ def set_rotation_controller_type(self, val): 1: SIMPLEPID_POS_CONTROLLER, 2: SIMPLEPID_VEL_CONTROLLER """ - return self.__moby_stub.SetRotationControllerType(IntVal(val=val)) + return self.__moby_stub.SetRotationControllerType(moby_msgs.IntVal(val=val)) @Utils.exception_handler def set_rotation_gain(self, index, k, kv, kp): """ Set Rotation Control Gain """ - return self.__moby_stub.SetControlParam(RotationGain(idx=index, k=k, kv=kv, kp=kp)) + return self.__moby_stub.SetControlParam(moby_msgs.RotationGain(idx=index, k=k, kv=kv, kp=kp)) @Utils.exception_handler def get_rotation_gain(self, index): """ Get Rotation Control Gain (k, kv, kp) """ - val = self.__moby_stub.GetControlParam(IntVal(val=index)) - return {'k': val.k, 'kv':val.kv, 'kp':val.kp} - + val = self.__moby_stub.GetControlParam(moby_msgs.IntVal(val=index)) + return {'k': val.k, 'kv': val.kv, 'kp': val.kp} + @Utils.exception_handler def set_kinematics_forced(self, onoff): """ Set Kinematics Forced( on=true, off=false ) """ - self.__moby_stub.SetForceKinematics(BoolVal(val=onoff)) + self.__moby_stub.SetForceKinematics(moby_msgs.BoolVal(val=onoff)) @Utils.exception_handler def get_kinematics_forced(self): """ Get Kinematics Forced( on=true, off=false ) """ - val = self.__moby_stub.GetForceKinematics(Empty()) + val = self.__moby_stub.GetForceKinematics(common_msgs.Empty()) return val ############################ # Moby-Agri related commands ############################ - + @Utils.exception_handler def turn_light(self, on): - return self.__moby_stub.TurnLightOnOff(BoolVal(val=on)) - + return self.__moby_stub.TurnLightOnOff(moby_msgs.BoolVal(val=on)) + @Utils.exception_handler def turn_buzzer(self, on): - return self.__moby_stub.TurnBuzzOnOff(BoolVal(val=on)) + return self.__moby_stub.TurnBuzzOnOff(moby_msgs.BoolVal(val=on)) @Utils.exception_handler def pause_bumper(self, on): - return self.__moby_stub.PauseBumper(BoolVal(val=on)) + return self.__moby_stub.PauseBumper(moby_msgs.BoolVal(val=on)) ############################ # Moby Data logging ############################ - + @Utils.exception_handler def start_rt_logging(self): """ Start RT logging """ - return self.__moby_stub.StartRTLogging(Empty()) - + return self.__moby_stub.StartRTLogging(common_msgs.Empty()) + @Utils.exception_handler def end_rt_logging(self): """ End RT logging """ - return self.__moby_stub.EndRTLogging(Empty()) - + return self.__moby_stub.EndRTLogging(common_msgs.Empty()) diff --git a/python/setup.py b/python/setup.py index 6bb0814..8e73691 100644 --- a/python/setup.py +++ b/python/setup.py @@ -12,24 +12,26 @@ "requests==2.22.0", "Pillow==9.5.0", "numpy==1.21.6", + "pandas-stubs<=2.0.1.230501", "pyModbusTCP==0.2.1", "netifaces==0.11.0" ] else: install_requires = [ - "grpcio", - "grpcio-tools", - "protobuf", - "requests", - "Pillow", - "numpy", - "pyModbusTCP", - "netifaces2" + "grpcio>=1.39.0,<=1.44.0", + "grpcio-tools>=1.39.0,<=1.44.0", + "protobuf>=3.17.3,<=3.20.3", + "requests>=2.22.0,<=2.32.3", + "Pillow==9.5.0", + "numpy==1.21.6", + "pandas-stubs<=2.0.1.230501", + "pyModbusTCP==0.2.1", + "netifaces2", ] setup( name="neuromeka", - version="3.2.0.6", + version="3.2.0.7", author="Neuromeka", author_email="technical-suuport@neuromeka.com", description="Neuromeka client protocols for IndyDCP3, IndyEye, Moby, Ecat, and Motor", @@ -42,13 +44,11 @@ "Intended Audience :: Developers", "License :: OSI Approved :: MIT License", "Operating System :: OS Independent", - "Programming Language :: Python :: 3", "Programming Language :: Python :: 3.6", "Programming Language :: Python :: 3.7", "Programming Language :: Python :: 3.8", - "Programming Language :: Python :: 3.9", - "Programming Language :: Python :: 3.10" + "Programming Language :: Python :: 3.9" ], python_requires=">=3.6", install_requires=install_requires, -) \ No newline at end of file +)