diff --git a/Controller/driver/driver.py b/Controller/driver/driver.py index 24a4e7d15bd6c652c4af4378609c03ae0c2ee68b..9b120d372e070a7396252f283acb4e8cafb9e333 100644 --- a/Controller/driver/driver.py +++ b/Controller/driver/driver.py @@ -600,6 +600,8 @@ class Driver: # PVT Move process error self.__process_error = None + self.__jmove = None + @property def enabled(self): """ @@ -1392,37 +1394,57 @@ class Driver: % (joint_ids, joints, velocity, acceleration)) try: - ids = self.get_joint_ids() - jmove = Trajectory("joints") - jmove.load({"joint_ids": ids}) - current = self.get_joints(ids, indeed=True) - target_joints = copy.deepcopy(current) - for i in range(len(ids)): - for j in range(len(joint_ids)): - if str(ids[i]) == str(joint_ids[j]): - target_joints[i] = joints[j] - if not self.__is_trajectory_supported[joint_ids[j]]: - # TODO 关节误差需要用一个更好的方式衡量 - if abs(current[i] - target_joints[i]) > 0.2: - raise ExecuteError(CONTROLLER_ERROR_DRIVER_EXECUTE, - "Joint %s does not support trajectory movement" % joint_ids[j]) - points = [current, target_joints] - # jmove.set(points, velocity, acceleration, PVT_DURATION, 1, False) - jmove.set(points, velocity, acceleration, PVT_DURATION, 1, False) - # Clear move process error - self.__process_error = None - # Start thread - move_thread = threading.Thread(target=self.move_process, args=(joint_ids, target_joints, None, None, - None, - jmove, 'movej', advance, )) - # move_thread.setDaemon(True) - move_thread.daemon = True - move_thread.start() - time.sleep(0.1) - self.__commander.wait_for_pvt_motion_start() + target_joints = None + if self.__jmove is None: + ids = self.get_joint_ids() + self.__jmove = Trajectory("joints") + self.__jmove.load({"joint_ids": ids}) + current = self.get_joints(ids, indeed=True) + target_joints = copy.deepcopy(current) + for i in range(len(ids)): + for j in range(len(joint_ids)): + if str(ids[i]) == str(joint_ids[j]): + target_joints[i] = joints[j] + if not self.__is_trajectory_supported[joint_ids[j]]: + # TODO 关节误差需要用一个更好的方式衡量 + if abs(current[i] - target_joints[i]) > 0.2: + raise ExecuteError(CONTROLLER_ERROR_DRIVER_EXECUTE, + "Joint %s does not support trajectory movement" % joint_ids[j]) + points = [current, target_joints] + # jmove.set(points, velocity, acceleration, PVT_DURATION, 1, False) + self.__jmove.set(points, velocity, acceleration, PVT_DURATION, 1, False) + # Clear move process error + self.__process_error = None + # # Start thread + # move_thread = threading.Thread(target=self.move_process, args=(joint_ids, target_joints, None, None, + # None, + # self.__jmove, 'movej', advance, )) + # # move_thread.setDaemon(True) + # move_thread.daemon = True + # move_thread.start() + # time.sleep(0.1) + # self.__commander.wait_for_pvt_motion_start() + time.sleep(0.001) + else: + target_joints = copy.deepcopy(joints) + self.__jmove.add(joints, velocity) + time.sleep(0.001) if block: + # generate trajectory + self.__jmove.generate() + # Start thread + move_thread = threading.Thread(target=self.move_process, args=(joint_ids, target_joints, None, None, + None, + self.__jmove, 'movej', advance, )) + # move_thread.setDaemon(True) + move_thread.daemon = True + move_thread.start() + time.sleep(0.1) + self.__commander.wait_for_pvt_motion_start() # Wait for motion stopping - self.__commander.wait_for_motors(ids, advance=advance) + # self.__commander.wait_for_motors(ids, advance=advance) + self.__commander.wait_for_motors(joint_ids, advance=advance) + self.__jmove = None if self.__process_error is not None: raise self.__process_error except BaseError as e: diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index 71db370da6a0d80b5229aad842bb1d76b4deb68f..b72eee7e9e97c8d02e4705f62e9167ad9fe6ce01 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -61,6 +61,8 @@ class Sync(threading.Thread): self.__target_velocity = 0 self.__target_acceleration = 0 self.__pvts = list() + self.last_velocity = 0.0 + self.zero_velocity = True # Config variable self.__motor_control_mode = self.__config.get('control_mode') self.__motion_complete_limit = self.__config.get('motion_complete_limit') @@ -296,7 +298,7 @@ class Sync(threading.Thread): self.__target_position = self.__position self.__target_velocity = self.__velocity last_position = copy.deepcopy(self.__position) - last_velocity = 0.0 + self.last_velocity = 0.0 duration = 0.01 self.__is_sync = True while self.__is_active: @@ -319,10 +321,23 @@ class Sync(threading.Thread): velocity = point[1] duration = copy.deepcopy(point[2]) last = point[4] - if velocity != 0: - last_velocity = copy.deepcopy(velocity) - else: - velocity = copy.deepcopy(last_velocity) + if velocity != 0 and self.__point_status != PVT_BUFFER_LOW: + if math.fabs(position - self.__position) > self.__motion_complete_limit * 3: + # positivity1 = 1 if self.__target_position - position >= 0 else -1 + # positivity2 = 1 if position - self.__position >= 0 else -1 + # if positivity1 == positivity2: + # self.last_velocity = copy.deepcopy(velocity) if math.fabs(velocity) > math.fabs(self.last_velocity) else copy.deepcopy(self.last_velocity) + # else: + # self.last_velocity = copy.deepcopy(velocity) if math.fabs(velocity) < math.fabs(self.last_velocity) else copy.deepcopy(self.last_velocity) + self.last_velocity = copy.deepcopy(velocity) if math.fabs(velocity) > math.fabs(self.last_velocity) else copy.deepcopy(self.last_velocity) + elif math.fabs(velocity) < 0.5 * math.fabs(self.last_velocity): + self.last_velocity = 0.5 * self.last_velocity + else: + self.last_velocity = copy.deepcopy(velocity) + # if self.__motor.id == "4": + # print(self.last_velocity) + # print("vel: %f", velocity) + velocity = copy.deepcopy(self.last_velocity) last_position = copy.deepcopy(position) send_data(position, velocity, self.__motor_kp, self.__motor_kd, self.__motor_tau) @@ -344,7 +359,7 @@ class Sync(threading.Thread): logger.error("Motor%s point queue size is zero" % self.__motor.id) time.sleep(self.DURATION) else: - process_sync_mode(last_velocity) + process_sync_mode(self.last_velocity) time.sleep(self.DURATION) else: if len(self.__pvts) > 0: @@ -352,16 +367,37 @@ class Sync(threading.Thread): self.__pvts.pop(0) target_velocity = pvt[1][0] target_position = pvt[0][0] - if target_velocity != 0: - last_velocity = copy.deepcopy(pvt[1][0]) - else: - target_velocity = copy.deepcopy(last_velocity) + if target_velocity != 0 : + # 电机实际运动与发送点位不匹配，基本当前位置超过了实际位置，速度一直无法取到高速 + # if math.fabs(target_position - self.__position) > self.__motion_complete_limit * 3: + # positivity1 = 1 if self.__target_position - target_position >= 0 else -1 + # positivity2 = 1 if target_position - self.__position >= 0 else -1 + # if positivity1 == positivity2: + # self.last_velocity = copy.deepcopy(pvt[1][0]) if math.fabs(pvt[1][0]) > math.fabs(self.last_velocity) else self.last_velocity + # else: + # self.last_velocity = copy.deepcopy(pvt[1][0]) if math.fabs(pvt[1][0]) < math.fabs(self.last_velocity) else self.last_velocity + # else: + # self.last_velocity = copy.deepcopy(pvt[1][0]) + if math.fabs(target_position - self.__position) > self.__motion_complete_limit * 3: + self.last_velocity = copy.deepcopy(pvt[1][0]) if math.fabs(pvt[1][0]) > math.fabs(self.last_velocity) else self.last_velocity + else: + self.last_velocity = copy.deepcopy(pvt[1][0]) + # 所取的数据和实际需要的数据 + # if self.__motor.id == "4": + # print(self.last_velocity) + # print("vel: %f", target_velocity) + target_velocity = copy.deepcopy(self.last_velocity) last_position = copy.deepcopy(pvt[0][0]) send_data(target_position, target_velocity, self.__motor_kp, self.__motor_kd, self.__motor_tau) time.sleep(self.DURATION) + if math.fabs(self.__target_position - self.__position) < self.__motion_complete_limit: + self.__motion_completed = True + self.__pvts.clear() + else: + self.__motion_completed = False else: - process_sync_mode(last_velocity) + process_sync_mode(self.last_velocity) time.sleep(self.DURATION) self.__is_sync = False @@ -399,6 +435,7 @@ class Sync(threading.Thread): :return: """ + self.last_velocity = 0.0 if math.fabs(self.__position - position) < self.__motion_complete_limit: return ids = [self.__motor.axis_id] @@ -406,6 +443,7 @@ class Sync(threading.Thread): jmove.load({"joint_ids": ids}) points = [[self.__position], [position]] jmove.set(points, self.__target_velocity, self.__target_acceleration, self.DURATION, 1, False) + jmove.generate() pvts = list() last_flag = list() while True: @@ -541,6 +579,7 @@ class Sync(threading.Thread): :return: """ + self.last_velocity = 0.0 self.__pvt_start = True self.__motion_completed = False @@ -576,6 +615,11 @@ class Sync(threading.Thread): :return: Flag on whether point was successfully sent. """ + if self.zero_velocity and velocity != 0.0: + self.zero_velocity = False + self.last_velocity = 0.0 + if not self.zero_velocity and velocity == 0.0: + self.zero_velocity = True if ((self.__point_count + 1) & MAX_PVT_COUNT) == count: self.__target_position = position self.__point_queue.put([position, velocity, duration, count, last]) @@ -622,9 +666,11 @@ class Sync(threading.Thread): :return: """ - self.__pvt_start = False - self.__point_queue.queue.clear() - self.__point_queue.put(None) + # movej停止时在commander：277行设置末位为True，不需要再这边停止 + # self.__pvt_start = False + # self.__point_queue.queue.clear() + # self.__point_queue.put(None) + pass class Motor(MotorBase): diff --git a/Controller/driver/trajectory/modules/joints/joints.py b/Controller/driver/trajectory/modules/joints/joints.py index a15ec8d11dc6e7cafaac0a541dbfef2bae38ac33..59f4421dc37807a69680ded967c259b9f856a176 100644 --- a/Controller/driver/trajectory/modules/joints/joints.py +++ b/Controller/driver/trajectory/modules/joints/joints.py @@ -12,6 +12,7 @@ from ..itrajectory import ITrajectory, TrajectoryInfo, TrajectoryPVT from Controller.utils.error import * from Controller.utils.unit import * +from Controller import logger # /********************************************************************************************************************/ # /*************************************** Joints Trajectory module ****************************************/ @@ -102,6 +103,7 @@ class Joints(ITrajectory): self.recent = 0.0 self.is_set = False + self.is_generate = False self.is_end = False self.looptime = 0.0 @@ -154,7 +156,8 @@ class Joints(ITrajectory): self.repeat = 1 self.segments = len(self.velocity) self.is_set = True - self.generate() + # self.generate() + self.is_generate = False def generate(self): """ @@ -327,6 +330,7 @@ class Joints(ITrajectory): for index in range(len(joint_ids)): self.sfunc[s][index].set_time_scale(tacc, self.interval[s] - tacc - tdec, tdec) + self.is_generate = True def add(self, poses, velocity): """ @@ -368,171 +372,171 @@ class Joints(ITrajectory): # self.generate() - # 动态添加 - s = self.segments - 1 - # 计算位置距离 - ns = (s + 1) % len(self.poses) - dist = list() - for index in range(len(joint_ids)): - dist.append(self.poses[ns][index] - self.poses[s][index]) - self.dists.append(dist) - - # 计算时间间隔 - maxt = 0 - for index in range(len(joint_ids)): - temp = math.fabs(self.dists[s][index] / self.velocity[s]) - if temp > maxt: - maxt = temp - self.interval_calc.append(maxt) - self.interval.append(maxt) - self.looptime += maxt - - if self.looptime < EPS: - raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ILLPARAM, "Illegal target pose") - - # 计算速度期望 - # 起点，开始速度为0 - if self.segments == 2: - self.start_section.clear() - for index in range(len(joint_ids)): - v = self.dists[0][index] / self.interval_calc[0] if self.interval_calc[0] != 0 else 0 - v3 = 0 - if self.segments > 1: - vns = (self.dists[1][index] / self.interval_calc[1]) if self.interval_calc[1] != 0 else 0 - if v * vns > 0: - v3 = (v + vns) / 2 - self.start_section.append(SecParam(self.dists[0][index], 0, v, v3)) - - # 终点，结尾速度为0 - self.end_section.clear() - for index in range(len(joint_ids)): - v1 = 0 - s = self.segments - 1 - v = self.dists[s][index] / self.interval_calc[s] if self.interval_calc[s] != 0 else 0 - if self.segments > 1: - ls = self.segments - 2 - vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 - if v * vls > 0: - v1 = (v + vls) / 2 - self.end_section.append(SecParam(self.dists[s][index], v1, v, 0)) - - # 循环部分 - self.sfunc.pop() - for s in range(self.segments - 2, self.segments): - sec = list() - for index in range(len(joint_ids)): - v1 = 0 - v = (self.dists[s][index] / self.interval_calc[s]) if self.interval_calc[s] != 0 else 0 - v3 = 0 - - ls = (s - 1) if s > 0 else self.segments - 1 - vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 - if v * vls > 0: - v1 = (v + vls) / 2 - - ns = (s + 1) % self.segments - vns = (self.dists[ns][index] / self.interval_calc[ns]) if self.interval_calc[ns] != 0 else 0 - if v * vns > 0: - v3 = (v + vns) / 2 - sec.append(SecParam(self.dists[s][index], v1, v, v3)) - self.sfunc.append(sec) - - # 计算 - for s in range(self.segments - 2, self.segments): - sec = list() - if s == 0: - sec = self.start_section - elif s == self.segments - 1: - sec = self.end_section - else: - sec = self.sfunc[s] - - tacc = 0.0 - tdec = 0.0 - for index in range(len(joint_ids)): - temp1 = math.fabs(sec[index].vt - sec[index].v1) / self.acceleration - temp2 = math.fabs(sec[index].vt - sec[index].v2) / self.acceleration - if tacc < temp1: - tacc = temp1 - if tdec < temp2: - tdec = temp2 - - temp = copy.deepcopy(self.interval_calc) - if self.interval_calc[s] < (tacc + tdec): - temp[s] = tacc + tdec - - for index in range(len(joint_ids)): - interval_ret = sec[index].set_time_scale(tacc, temp[s] - tacc - tdec, tdec) - if interval_ret > self.interval_calc[s]: - self.interval[s] = interval_ret - - # 起始点处理 - if self.segments == 2: - tacc = 0.0 - tdec = 0.0 - for index in range(len(joint_ids)): - temp1 = math.fabs(self.start_section[index].vt - self.start_section[index].v1) / self.acceleration - temp2 = math.fabs(self.start_section[index].vt - self.start_section[index].v2) / self.acceleration - if tacc < temp1: - tacc = temp1 - if tdec < temp2: - tdec = temp2 - - if tacc == 0 and self.interval[0] < tdec: - tdec = self.interval[0] - elif self.interval[0] < tacc and tdec == 0: - tacc = self.interval[0] - else: - if self.interval[0] < (tacc + tdec): - self.interval[0] = tacc + tdec - - for index in range(len(joint_ids)): - self.start_section[index].set_time_scale(tacc, self.interval[0] - tacc - tdec, tdec) - - # 终止点处理 - tacc = 0.0 - tdec = 0.0 - for index in range(len(joint_ids)): - temp1 = math.fabs(self.end_section[index].vt - self.end_section[index].v1) / self.acceleration - temp2 = math.fabs(self.end_section[index].vt - self.end_section[index].v2) / self.acceleration - if tacc < temp1: - tacc = temp1 - if tdec < temp2: - tdec = temp2 - - if tacc == 0 and self.interval[self.segments - 1] < tdec: - tdec = self.interval[self.segments - 1] - elif self.interval[self.segments - 1] < tacc and tdec == 0: - tacc = self.interval[self.segments - 1] - else: - if self.interval[self.segments - 1] < (tacc + tdec): - self.interval[self.segments - 1] = tacc + tdec - - for index in range(len(joint_ids)): - self.end_section[index].set_time_scale(tacc, self.interval[self.segments - 1] - tacc - tdec, tdec) - - # 计算 - for s in range(self.segments - 2, self.segments): - tacc = 0.0 - tdec = 0.0 - for index in range(len(joint_ids)): - temp1 = math.fabs(self.sfunc[s][index].vt - self.sfunc[s][index].v1) / self.acceleration - temp2 = math.fabs(self.sfunc[s][index].vt - self.sfunc[s][index].v2) / self.acceleration - if tacc < temp1: - tacc = temp1 - if tdec < temp2: - tdec = temp2 - - if tacc == 0 and self.interval[s] < tdec: - tdec = self.interval[s] - elif self.interval[s] < tacc and tdec == 0: - tacc = self.interval[s] - else: - if self.interval[s] < (tacc + tdec): - self.interval[s] = tacc + tdec - - for index in range(len(joint_ids)): - self.sfunc[s][index].set_time_scale(tacc, self.interval[s] - tacc - tdec, tdec) + # # 动态添加 + # s = self.segments - 1 + # # 计算位置距离 + # ns = (s + 1) % len(self.poses) + # dist = list() + # for index in range(len(joint_ids)): + # dist.append(self.poses[ns][index] - self.poses[s][index]) + # self.dists.append(dist) + + # # 计算时间间隔 + # maxt = 0 + # for index in range(len(joint_ids)): + # temp = math.fabs(self.dists[s][index] / self.velocity[s]) + # if temp > maxt: + # maxt = temp + # self.interval_calc.append(maxt) + # self.interval.append(maxt) + # self.looptime += maxt + + # if self.looptime < EPS: + # raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ILLPARAM, "Illegal target pose") + + # # 计算速度期望 + # # 起点，开始速度为0 + # if self.segments == 2: + # self.start_section.clear() + # for index in range(len(joint_ids)): + # v = self.dists[0][index] / self.interval_calc[0] if self.interval_calc[0] != 0 else 0 + # v3 = 0 + # if self.segments > 1: + # vns = (self.dists[1][index] / self.interval_calc[1]) if self.interval_calc[1] != 0 else 0 + # if v * vns > 0: + # v3 = (v + vns) / 2 + # self.start_section.append(SecParam(self.dists[0][index], 0, v, v3)) + + # # 终点，结尾速度为0 + # self.end_section.clear() + # for index in range(len(joint_ids)): + # v1 = 0 + # s = self.segments - 1 + # v = self.dists[s][index] / self.interval_calc[s] if self.interval_calc[s] != 0 else 0 + # if self.segments > 1: + # ls = self.segments - 2 + # vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 + # if v * vls > 0: + # v1 = (v + vls) / 2 + # self.end_section.append(SecParam(self.dists[s][index], v1, v, 0)) + + # # 循环部分 + # self.sfunc.pop() + # for s in range(self.segments - 2, self.segments): + # sec = list() + # for index in range(len(joint_ids)): + # v1 = 0 + # v = (self.dists[s][index] / self.interval_calc[s]) if self.interval_calc[s] != 0 else 0 + # v3 = 0 + + # ls = (s - 1) if s > 0 else self.segments - 1 + # vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 + # if v * vls > 0: + # v1 = (v + vls) / 2 + + # ns = (s + 1) % self.segments + # vns = (self.dists[ns][index] / self.interval_calc[ns]) if self.interval_calc[ns] != 0 else 0 + # if v * vns > 0: + # v3 = (v + vns) / 2 + # sec.append(SecParam(self.dists[s][index], v1, v, v3)) + # self.sfunc.append(sec) + + # # 计算 + # for s in range(self.segments - 2, self.segments): + # sec = list() + # if s == 0: + # sec = self.start_section + # elif s == self.segments - 1: + # sec = self.end_section + # else: + # sec = self.sfunc[s] + + # tacc = 0.0 + # tdec = 0.0 + # for index in range(len(joint_ids)): + # temp1 = math.fabs(sec[index].vt - sec[index].v1) / self.acceleration + # temp2 = math.fabs(sec[index].vt - sec[index].v2) / self.acceleration + # if tacc < temp1: + # tacc = temp1 + # if tdec < temp2: + # tdec = temp2 + + # temp = copy.deepcopy(self.interval_calc) + # if self.interval_calc[s] < (tacc + tdec): + # temp[s] = tacc + tdec + + # for index in range(len(joint_ids)): + # interval_ret = sec[index].set_time_scale(tacc, temp[s] - tacc - tdec, tdec) + # if interval_ret > self.interval_calc[s]: + # self.interval[s] = interval_ret + + # # 起始点处理 + # if self.segments == 2: + # tacc = 0.0 + # tdec = 0.0 + # for index in range(len(joint_ids)): + # temp1 = math.fabs(self.start_section[index].vt - self.start_section[index].v1) / self.acceleration + # temp2 = math.fabs(self.start_section[index].vt - self.start_section[index].v2) / self.acceleration + # if tacc < temp1: + # tacc = temp1 + # if tdec < temp2: + # tdec = temp2 + + # if tacc == 0 and self.interval[0] < tdec: + # tdec = self.interval[0] + # elif self.interval[0] < tacc and tdec == 0: + # tacc = self.interval[0] + # else: + # if self.interval[0] < (tacc + tdec): + # self.interval[0] = tacc + tdec + + # for index in range(len(joint_ids)): + # self.start_section[index].set_time_scale(tacc, self.interval[0] - tacc - tdec, tdec) + + # # 终止点处理 + # tacc = 0.0 + # tdec = 0.0 + # for index in range(len(joint_ids)): + # temp1 = math.fabs(self.end_section[index].vt - self.end_section[index].v1) / self.acceleration + # temp2 = math.fabs(self.end_section[index].vt - self.end_section[index].v2) / self.acceleration + # if tacc < temp1: + # tacc = temp1 + # if tdec < temp2: + # tdec = temp2 + + # if tacc == 0 and self.interval[self.segments - 1] < tdec: + # tdec = self.interval[self.segments - 1] + # elif self.interval[self.segments - 1] < tacc and tdec == 0: + # tacc = self.interval[self.segments - 1] + # else: + # if self.interval[self.segments - 1] < (tacc + tdec): + # self.interval[self.segments - 1] = tacc + tdec + + # for index in range(len(joint_ids)): + # self.end_section[index].set_time_scale(tacc, self.interval[self.segments - 1] - tacc - tdec, tdec) + + # # 计算 + # for s in range(self.segments - 2, self.segments): + # tacc = 0.0 + # tdec = 0.0 + # for index in range(len(joint_ids)): + # temp1 = math.fabs(self.sfunc[s][index].vt - self.sfunc[s][index].v1) / self.acceleration + # temp2 = math.fabs(self.sfunc[s][index].vt - self.sfunc[s][index].v2) / self.acceleration + # if tacc < temp1: + # tacc = temp1 + # if tdec < temp2: + # tdec = temp2 + + # if tacc == 0 and self.interval[s] < tdec: + # tdec = self.interval[s] + # elif self.interval[s] < tacc and tdec == 0: + # tacc = self.interval[s] + # else: + # if self.interval[s] < (tacc + tdec): + # self.interval[s] = tacc + tdec + + # for index in range(len(joint_ids)): + # self.sfunc[s][index].set_time_scale(tacc, self.interval[s] - tacc - tdec, tdec) def get(self): @@ -545,6 +549,9 @@ class Joints(ITrajectory): info = TrajectoryInfo() if not self.is_set: raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Trajectory generator is not set") + + if not self.is_generate: + raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Trajectory generator is not generated") if 0 < self.repeat < self.cycle: raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Trajectory generator out of repeat range") @@ -561,7 +568,10 @@ class Joints(ITrajectory): for index in range(len(nsection)): p, v = nsection[index].get(self.tnow) if p is None or v is None: - raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Generate position and velocity value failed") + # raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Generate position and velocity value failed") + logger.error("Generate position and velocity value failed") + info.success = False + return None, info pose.append(TrajectoryPVT(p + self.poses[self.section][index], v, self.recent)) info.depart_point = self.section @@ -604,6 +614,7 @@ class Joints(ITrajectory): :return: """ self.is_set = False + self.is_generate = False self.is_end = False self.section = 0 self.cycle = 1 @@ -626,6 +637,7 @@ class Joints(ITrajectory): self.end_section.clear() self.is_end = False + self.is_generate = False self.section = 0 self.cycle = 1 self.tnow = 0.0 diff --git a/Controller/driver/trajectory/trajectory.py b/Controller/driver/trajectory/trajectory.py index 1e295c85b39061c31d25e6d93990b16533c816cb..c1d29aabfb7e547d2db12b433ed7e03e22411d30 100644 --- a/Controller/driver/trajectory/trajectory.py +++ b/Controller/driver/trajectory/trajectory.py @@ -84,6 +84,20 @@ class Trajectory: except Exception as err: error_message = "Trajectory set poses with error: %s" % repr(err) raise ExecuteError(CONTROLLER_ERROR_TRAJECTORY_EXEC, error_message) + + def generate(self): + """ + Generate trajectory + + :return: + """ + try: + return self.__trajectory.generate() + except BaseError as err: + raise err + except Exception as err: + error_message = "Trajectory generate with error: %s" % repr(err) + raise ExecuteError(CONTROLLER_ERROR_TRAJECTORY_EXEC, error_message) def get(self): """ diff --git a/README.md b/README.md index 2421a1dda1b2c45e7a00d869edca1e6bb1c10ac3..859ab130ab81cfb644919a05b142257b9c4f07f5 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,12 @@ Coarm机械臂底层运动控制库 ## 更新说明 +### Update v1.0.15 +1. trajectory 的set部分和generate部分分离 +2. movej增加轨迹的add操作（需要在block=True时才能生成轨迹） +3. 优化motor驱动程序 +4. 优化轨迹joints生成时出现Generate position and velocity value failed错误后无法返回指令 + ### Update v1.0.14 1. 修改motor下判断 2. 添加joints的add操作