diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index dbf660b..76ab842 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -9,7 +9,7 @@ robot: digital_twin: tcp_gizmo: - enabled: true # TCP-гизма в Webots; только в режиме симуляции + enabled: false # TCP-гизма в Webots; только в режиме симуляции publish_period: 0.032 # период управления [с] command_timeout: 0.3 # максимальный возраст цели при отпускании мыши [с] state_timeout: 0.5 # допустимый возраст состояния робота [с] @@ -23,7 +23,7 @@ digital_twin: controller_timer: "50" objects: - enabled: true + enabled: false count: 5 # точное общее количество; модели могут повторяться models: - path: pkg://iiwa_description/objects/hammer/hammer1.obj diff --git a/src/iiwa_experement/iiwa_experement/motion.py b/src/iiwa_experement/iiwa_experement/motion.py index 67efd4b..f67e7fe 100644 --- a/src/iiwa_experement/iiwa_experement/motion.py +++ b/src/iiwa_experement/iiwa_experement/motion.py @@ -357,6 +357,21 @@ class MotionAdapter: True, 'motion completed', label, True, base_goal, actual, time.monotonic() - started, ) + # The Webots trajectory controller reports success on the desired + # trajectory state while the measured arm is still creeping to the + # held final setpoint. Give it a bounded window to arrive before + # re-issuing the goal; re-issuing at once looks like a violent + # sideways snap of 5-10 cm. + converged = self._wait_for_converged_pose(base_goal, timeout_sec=8.0) + if converged is not None: + if self.node is not None: + self.node.get_logger().info( + f'{label}: tcp converged to the goal after a short wait' + ) + return MotionResult( + True, 'motion completed', label, True, base_goal, converged, + time.monotonic() - started, + ) if attempt + 1 < self.pose_correction_attempts: # The action succeeded but the controller stopped outside the # pose tolerance. Re-issue the same verified goal once; this @@ -526,6 +541,30 @@ class MotionAdapter: True, None, self.actual_tcp(), time.monotonic() - started, ) + def _wait_for_converged_pose( + self, goal: Transform, timeout_sec: float, + ) -> Transform | None: + """Wait for the measured tcp to reach the goal within tolerances. + + Returns the converged pose, or None when the window elapsed. + """ + if self.node is None: + return None + import rclpy + + deadline = time.monotonic() + max(0.0, float(timeout_sec)) + while time.monotonic() < deadline: + rclpy.spin_once(self.node, timeout_sec=0.05) + current = self.actual_tcp() + if current is not None: + distance, angle = _pose_distance(current, goal) + if ( + distance <= self.position_tolerance + and angle <= self.orientation_tolerance + ): + return current + return None + def _settle(self) -> None: if self.settle_time_sec <= 0.0 or self.node is None: return diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index 2bdb889..e330dab 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/src/iiwa_experement/iiwa_experement/operations.py @@ -739,6 +739,7 @@ class ReferenceOperations: twist_error = None twist_done = False joint_twist = False + twisted_goal = goal # Prefer a pure flange rotation: it twists the tool about its own # axis without moving the arm to another IK branch. A Cartesian # twist can push joint 6 into its limit, after which 'work' is no @@ -782,6 +783,7 @@ class ReferenceOperations: f'tighten_hole({hole_id}):{suffix}', ) twist_done = True + twisted_goal = candidate break except OperationError as error: # Holes on the far side of the body can sit against @@ -793,14 +795,24 @@ class ReferenceOperations: twist_error = error if not twist_done: raise twist_error - # Return the flange to the contact orientation before leaving. - # Without the untwist, joint 7 accumulates 45 deg per hole and the - # wrist winds up until every later plan starts from a twisted - # branch. + # Lift axially out of the hole BEFORE undoing the twist: rotating + # the flange back at the contact point would visibly unscrew the + # simulated tightening. + self._retreat(twisted_goal, f'tighten_hole({hole_id})') if joint_twist: + # Return only joint 7 to its pre-twist value; the other + # joints stay where the retreat left them. + current = provider() if provider is not None else None + self._require( + current is not None and len(current) == 7, + f'tighten_hole({hole_id}): joint state unavailable for ' + 'the untwist', + ) + untwist_joints = list(current) + untwist_joints[6] = joints[6] self.motion.wait_stationary() untwist = self.motion.move_joints( - joints, self.geometry.speeds['contour'], + untwist_joints, self.geometry.speeds['contour'], f'tighten_hole({hole_id}):joint_untwist', ) if not getattr(untwist, 'success', False): @@ -809,13 +821,13 @@ class ReferenceOperations: f'{untwist.message}' ) else: + # Pure rotation about the tool axis at the safe height; the + # raised untwisted pose shares the retreat position. self._move( - goal, 'lin', self.geometry.speeds['contour'], + self._above(goal, self.geometry.transfer_gap), 'lin', + self.geometry.speeds['contour'], f'tighten_hole({hole_id}):untwist', ) - # Lift axially out of the hole before the route continues, exactly - # as after a screw handoff. - self._retreat(goal, f'tighten_hole({hole_id})') self._emit('tighten_simulated', object=hole_id, phase='done', twist_deg=twist_deg) return OperationResult( diff --git a/src/iiwa_experement/iiwa_experement/runtime_config.py b/src/iiwa_experement/iiwa_experement/runtime_config.py index fcf51b5..58beec9 100644 --- a/src/iiwa_experement/iiwa_experement/runtime_config.py +++ b/src/iiwa_experement/iiwa_experement/runtime_config.py @@ -131,6 +131,13 @@ def _materialize_setting( objects['enabled'] = False objects['count'] = 0 webots['world'] = str(world_path.resolve()) + # The interactive TCP gizmo must not exist in reference runs: it draws an + # overlay ball and can preempt the reference route. cobot-setting.yaml + # only applies to `cobot run`, not to this launch. + gizmo = _mapping( + digital_twin.setdefault('tcp_gizmo', {}), 'digital_twin.tcp_gizmo', + ) + gizmo['enabled'] = False tool = _mapping(data.setdefault('tool', {}), 'tool') tool['active'] = 'patron' diff --git a/src/iiwa_experement/worlds/grant/.iiwa-grant.wbproj b/src/iiwa_experement/worlds/grant/.iiwa-grant.wbproj new file mode 100644 index 0000000..0f71cc1 --- /dev/null +++ b/src/iiwa_experement/worlds/grant/.iiwa-grant.wbproj @@ -0,0 +1,9 @@ +Webots Project File version R2025a +perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a000002effc0200000001fb0000001400540065007800740045006400690074006f00720000000014000002ef0000003f00ffffff0000000300000780000000dafc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c010000000000000780000000000000000000000780000003cb00000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff0000000100000002000000000000077e0100000002010000000101 +sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000000100000002010000000201 +maximizedDockId: -1 +centralWidgetVisible: 1 +renderingMode: PLAIN +orthographicViewHeight: 1 +textFiles: -1