Move It IK returns configurations

compas-dev · Nov 4, 2024 · 8529997 · 8529997
1 parent 98dd481
commit 8529997
Show file tree

Hide file tree

Showing 6 changed files with 37 additions and 148 deletions.
diff --git a/docs/examples/03_backends_ros/files/01_ros_test.py b/docs/examples/03_backends_ros/files/01_ros_test.py
diff --git a/docs/examples/03_backends_ros/files/03_inverse_kinematics.py b/docs/examples/03_backends_ros/files/03_inverse_kinematics.py
@@ -11,10 +11,20 @@
     assert robot_cell.robot_model.name == "ur5_robot"
     planner = MoveItPlanner(client)
 
-    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    # Create a default RobotCellState from the RobotCell as the starting state
+    start_state = RobotCellState.from_robot_cell(robot_cell)
+
+    # Create a target frame with the ROBOT mode
+    frame_WCF = Frame([0.3, 0.1, 0.5], [0, -1, 0], [0, 0, -1])
     target = FrameTarget(frame_WCF, TargetMode.ROBOT)
+    print("Target frame with Robot Mode: {}".format(target))
 
-    start_state = RobotCellState.from_robot_cell(robot_cell)
+    # Calculate inverse kinematics
     configuration = planner.inverse_kinematics(target, start_state)
-
     print("Found configuration", configuration)
+
+    # Check that the forward kinematics of the found configuration matches the target frame
+    start_state.robot_configuration = configuration
+    frame = planner.forward_kinematics(start_state, TargetMode.ROBOT)
+    print("Robot's Planner Coordinate Frame (PCF)")
+    print(frame)
diff --git a/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py b/docs/examples/03_backends_ros/files/03_iter_inverse_kinematics.py
@@ -11,7 +11,7 @@
     assert robot_cell.robot_model.name == "ur5_robot"
     planner = MoveItPlanner(client)
 
-    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    frame_WCF = Frame([0.3, 0.1, 0.5], [0, -1, 0], [0, 0, -1])
     target = FrameTarget(frame_WCF, TargetMode.ROBOT)
 
     start_state = RobotCellState.from_robot_cell(robot_cell)

diff --git a/docs/examples/03_backends_ros/files/04_plan_cartesian_motion_with_attached_objects.py b/docs/examples/03_backends_ros/files/04_plan_cartesian_motion_with_attached_objects.py
@@ -12,6 +12,7 @@
 from compas_fab.robots import RobotCellState
 from compas_fab.robots import FrameWaypoints
 from compas_fab.robots import TargetMode
+from compas_fab.robots import FrameTarget
 from compas_fab.robots import RigidBodyLibrary
 
 target_frames = []
@@ -68,13 +69,25 @@
     # Place target marker at location
     for i, frame in enumerate(target_frames):
         robot_cell_state.rigid_body_states["target_%d" % i].frame = frame
-    # Initial configuration of the robot
-    robot_cell_state.robot_configuration.joint_values = [0, pi / -2, pi / 2, 0, 0, 0]
     result = planner.set_robot_cell(robot_cell, robot_cell_state)
 
-    fk_frame = planner.forward_kinematics(robot_cell_state, TargetMode.WORKPIECE)
-    print(fk_frame)
-    input("Press Enter to continue...")
+    # ===============================================
+    # Step 3: Perform IK to get initial configuration
+    # ===============================================
+
+    # robot_cell_state.robot_configuration.joint_values = [0, pi / -2, pi / 2, 0, 0, 0]
+
+    # fk_frame = planner.forward_kinematics(robot_cell_state, TargetMode.WORKPIECE)
+    # print(fk_frame)
+    # input("Press Enter to continue...")
+
+    # It is a common pattern to find the robot's starting configuration by solving the inverse kinematics
+    # for the first target frame.
+    configuration = planner.inverse_kinematics(
+        FrameTarget(target_frames[0], TargetMode.WORKPIECE), robot_cell_state, options=dict(avoid_collisions=False)
+    )
+    robot_cell_state.robot_configuration = configuration
+    print("Initial configuration: ", configuration)
 
     # =====================================
     # Step 3: Plan Cartesian Motion

diff --git a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py
@@ -327,7 +327,11 @@ def _iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group,
 
             # If result is unique, we yield it back to the user
             if result_is_unique:
-                yield joint_positions, joint_names
+                return_full_configuration = options.get("return_full_configuration", False)
+                configuration = planner._build_configuration(
+                    joint_positions, joint_names, group, return_full_configuration, start_configuration
+                )
+                yield configuration
                 all_yielded_joint_positions.append(joint_positions)
 
             # Generate random start configuration for next iteration

diff --git a/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py b/src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py
@@ -177,7 +177,6 @@ def plan_cartesian_motion_with_frame_waypoints_async(
 
         def response_handler(response):
             try:
-                print("Error Code:", response.error_code)
                 trajectory = convert_trajectory(
                     joints, response.solution, response.start_state, response.fraction, None, response
                 )