location of pickup works

tufts-ai-robotics-group · Jul 7, 2023 · e4cda37 · e4cda37
1 parent d82f35c
commit e4cda37
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 6 deletions.
diff --git a/scripts/obtain_pick_item_pose.py b/scripts/obtain_pick_item_pose.py
@@ -9,8 +9,9 @@
 ##################################################################
 
 import rospy
-from geometry_msgs.msg import TransformStamped, PoseStamped
+from geometry_msgs.msg import TransformStamped, PoseStamped, Quaternion
 from gazebo_msgs.msg import ModelStates
+from tf.transformations import quaternion_from_euler
 import tf2_ros
 import tf2_geometry_msgs
 
@@ -56,7 +57,9 @@ def get_pose(self):
         while True:
             try:
                 target_transform: TransformStamped = self.tf_buffer.lookup_transform(LOCOBOT_TF, WORLD_TF, rospy.Time(0))
-                return tf2_geometry_msgs.do_transform_pose(self.object_pose, target_transform)
+                target_pose = tf2_geometry_msgs.do_transform_pose(self.object_pose, target_transform).pose
+                target_pose.orientation = Quaternion(*quaternion_from_euler(ai=0, aj=0.8, ak=0))
+                return target_pose
             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                 rate.sleep()
 

diff --git a/scripts/universal_move_arm.py b/scripts/universal_move_arm.py
@@ -166,10 +166,10 @@ def main():
 
     # go to some valid pose
     # arm_group.go_to_pose_goal()
-    arm_group.go_to_pose_goal(Pose(
-        position=Point(x=0.5, y=0, z=0.35),
-        orientation=Quaternion(x=0, y=0, z=0, w=1)
-    ))
+    # arm_group.go_to_pose_goal(Pose(
+    #     position=Point(x=0.5, y=0, z=0.35),
+    #     orientation=Quaternion(x=0, y=0, z=0, w=1)
+    # ))
 
     # go to the location of a specific object
     pose_calc = PickUpPoseCalculator("cricket_ball")