diff --git a/main.py b/main.py index 8e746e7..3bd6555 100755 --- a/main.py +++ b/main.py @@ -266,7 +266,7 @@ if __name__ == "__main__": left_pick = otc.tf_lookup('a1') left_pick_angles = euler_from_quaternion([left_pick.orientation.x , left_pick.orientation.y , left_pick.orientation.z, left_pick.orientation.w]) - xangle = left_pick_angles[0] #+ radians(90) + xangle = left_pick_angles[0] + radians(90) left_pick_angles = (xangle, left_pick_angles[1], left_pick_angles[2]) target_quat = quaternion_from_euler(left_pick_angles[0], left_pick_angles[1], left_pick_angles[2]) left_pick_proc = Pose() @@ -283,10 +283,11 @@ if __name__ == "__main__": left_place = otc.tf_lookup('f2') - left_place_engles = euler_from_quaternion([left_place.orientation.x , left_place.orientation.y , left_place.orientation.z, left_place.orientation.w]) - xangle = left_place_engles[0] #+ radians(90) - left_place_engles = (xangle, left_place_engles[1], left_place_engles[2]) - target_quat = quaternion_from_euler(left_place_engles[0], left_place_engles[1], left_pick_angles[2]) + left_place_angles = euler_from_quaternion([left_place.orientation.x , left_place.orientation.y , left_place.orientation.z, left_place.orientation.w]) + xangle = left_place_angles[0] + radians(90) + xangle2 = left_place_angles[2] + radians(90) + left_place_angles = (xangle, left_place_angles[1], left_place_angles[2]) + target_quat = quaternion_from_euler(left_place_angles[0], left_place_angles[1], left_pick_angles[2]) left_place_pos = Pose() left_place_pos.position.x = left_place.position.x - 0.05 left_place_pos.position.y = left_place.position.y