diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index 854a253..9857a44 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -1336,6 +1336,52 @@ class MassageRobot: result_points.append(copy.deepcopy(center_point)) return result_points + def user_adjust(self,pose_increment = None, force = None, temperature = None, speed = None, direction = None ,gear = None, shake = None,press = None,frequency = None): + self.logger.log_info("用户调整") + if force is not None: + self.adjust_wrench = np.array(force) + self.adjust_wrench_envent.set() + if temperature is not None: + if self.thermotherapy is not None: + self.thermotherapy.set_working_status(heat_level=temperature) + elif self.stone is not None: + self.stone.set_working_status(heat_level=temperature) + elif self.ion is not None: + self.ion.set_working_status(goal_state=temperature) + elif self.heat is not None: + self.heat.set_working_status(heat_level=temperature) + if speed is not None: + if self.stone is not None: + self.stone.set_working_status(speed_level=speed) + if direction is not None: + if self.stone is not None: + self.stone.set_working_status(rotate_direction=direction) + if gear is not None: + if self.thermotherapy is not None: + self.thermotherapy.set_working_status(cur_level=gear) + if shake is not None: + if self.thermotherapy is not None: + self.thermotherapy.set_working_status(vib_level=shake) + if press is not None: + if self.shockwave is not None: + self.shockwave.p_set(level=press) + if frequency is not None: + if self.shockwave is not None: + self.shockwave.f_set(level=frequency) + + def user_interrupt(self): + self.logger.log_info("用户打断") + self.interrupt_event.set() + + def user_pause(self): + self.logger.log_info("用户暂停") + self.pause_envent.set() + + def user_skip(self): + self.logger.log_info("用户跳过") + self.skip_envent.set() + + def traj_generate(self, t: Union[int, float, List[float]], pose = None, wrench = None, interpolation: Literal["linear", "cubic","circle","cloud_point"] = 'linear',**kwargs): """ 轨迹生成