Fix I-Marker Rotation for Pick Object (#290)

* rotate 180 degrees about x-axis Signed-off-by: Paul Gesel <[email protected]> * Update src/picknik_ur_site_config/objectives/pick_object.xml Co-authored-by: Abishalini Sivaraman <[email protected]> --------- Signed-off-by: Paul Gesel <[email protected]> Co-authored-by: Abishalini Sivaraman <[email protected]>
PickNikRobotics · Jun 21, 2024 · b5091e2 · b5091e2
1 parent 6b1517e
commit b5091e2
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/src/picknik_ur_site_config/objectives/pick_object.xml b/src/picknik_ur_site_config/objectives/pick_object.xml
@@ -8,7 +8,13 @@
                 <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="grasp_pose" point_prompts="Select the object you wish to pick" pixel_coords="{pixel_coords}"/>
                 <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
                 <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
-                <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{output_poses}" adjusted_poses="{adjusted_poses}"/>
+                <Decorator ID="ForEachPoseStamped" vector_in="{output_poses}">
+                    <Control ID="Sequence">
+                        <Action ID="TransformPose" quaternion_xyzw="1;0;0;0" translation_xyz="0;0;0" input_pose="{pose}" output_pose="{output_poses_flipped}"/>
+                        <Action ID="AddPoseStampedToVector" input="{output_poses_flipped}" vector="{pose_stamped_vector}"/>
+                    </Control>
+                </Decorator>
+                <Action ID="AdjustPoseWithIMarker" prompts="Adjust IMarker to desired pose" initial_poses="{pose_stamped_vector}" adjusted_poses="{adjusted_poses}"/>
             </Control>
             <Decorator ID="ForEachPoseStamped" vector_in="{adjusted_poses}" out="{grasp_pose}">
                 <Control ID="Sequence">