sucessful full motion sequence

Author: theodom

src/percussion_motion/percussion_motion/percussion_motion_node.py

@@ -91,7 +91,7 @@ def __init__(self) -> None:
 
         # --- Parameters ---
         self.declare_parameter('robot_ip',         '169.254.0.22')
-        self.declare_parameter('home_pose', [-0.0068470948161484385, 0.43908904189889597, 0.5208511217908346, 1.9216815074629128, 0.7705997896029447, 1.5926581228860712]
+        self.declare_parameter('home_pose', [0.0652, 0.4498, 0.4911, 1.9767, 0.9942, 1.6870]
 )
         self.declare_parameter('default_velocity', 0.2)
         self.declare_parameter('default_accel',    0.2)

src/percussion_motion/percussion_motion/rtde_motions.py

@@ -122,12 +122,22 @@ def move_to_pose(
     ----------
     target_pose : [x, y, z, rx, ry, rz]  in the robot base frame
     """
+
+    current_Q = rtde_r.getActualQ()
     try:
-        rtde_c.moveJ_IK(target_pose, velocity, acceleration, blend)
-        final = _current_tcp(rtde_r)
-        return MoveResult(MoveStatus.SUCCESS, 'Reached target pose.', final)
-    except Exception as exc:
-        return MoveResult(MoveStatus.FAILED, f'moveJ_IK failed: {exc}')
+        target_Q = rtde_c.getInverseKinematics(target_pose, current_Q)
+    except Exception as e:
+        return MoveResult(MoveStatus.FAILED, f'IK failed: {e}')
+    except len(target_Q) != 6:
+        return MoveResult(MoveStatus.FAILED, f'IK failed: no Q vector returned')
+    try:
+        rtde_c.moveJ(target_Q, velocity, acceleration, blend)
+    except Exception as e:
+        return MoveResult(MoveStatus.FAILED, f'moveJ failed: {e}')
+    # rtde_c.moveJ_IK(target_pose, velocity, acceleration, blend)
+    final = _current_tcp(rtde_r)
+    return MoveResult(MoveStatus.SUCCESS, 'Reached target pose.', final)
+
 
 
 def move_until_contact(

src/percussion_task_manager/percussion_task_manager/task_manager_node.py

@@ -83,7 +83,7 @@ def _build_sequence(self, marker_pose: Pose6D) -> List[dict]:
             {
                 'motion_type':    'RELATIVE_MOVE', # move backwards from Ledger + UP
                 'marker_pose':    _make_pose6d(),
-                'approach_offset': [0, 0.12, -0.05, 0, 0.0, 0.0], # TCP frame
+                'approach_offset': [0, 0.15, -0.05, 0, 0.0, 0.0], # TCP frame
             },
             {
                 'motion_type':    'RELATIVE_MOVE', # Rotate Tool to face wedgelock
@@ -93,7 +93,7 @@ def _build_sequence(self, marker_pose: Pose6D) -> List[dict]:
             {
                 'motion_type':    'RELATIVE_MOVE', # Move towards wedgelock (sideways) for contact 2D
                 'marker_pose':    _make_pose6d(),
-                'approach_offset': [0.080, 0, 0, 0, 0.0, 0.0],  # TCP frame
+                'approach_offset': [0.080, -0.05, 0, 0, 0.0, 0.0],  # TCP frame
             },
             {
                 'motion_type': 'MOVE_TO_CONTACT', # Touch ledger top down
@@ -118,7 +118,7 @@ def _build_sequence(self, marker_pose: Pose6D) -> List[dict]:
             {
                 'motion_type':    'RELATIVE_MOVE', # Move into striking position
                 'marker_pose':    _make_pose6d(),
-                'approach_offset': [0.0, 0.0, -0.00200, 0.0, 0.0, 0.0],   # TCP frame
+                'approach_offset': [0.0, 0.0, -0.00150, 0.0, 0.0, 0.0],   # TCP frame
             },
         ]