IRA Task Space test

#!/usr/bin/env python
import rospy
import csv
from open_manipulator_msgs.srv import SetKinematicsPose, SetKinematicsPoseRequest

def test_pose(x, y, z):
    req = SetKinematicsPoseRequest()
    req.planning_group = "arm"
    req.end_effector_name = "gripper"
    req.kinematics_pose.pose.position.x = x
    req.kinematics_pose.pose.position.y = y
    req.kinematics_pose.pose.position.z = z
    req.kinematics_pose.pose.orientation.x = 0.0
    req.kinematics_pose.pose.orientation.y = 0.0
    req.kinematics_pose.pose.orientation.z = 0.0
    req.kinematics_pose.pose.orientation.w = 1.0
    req.path_time = 1.5

    try:
        result = move_service(req)
        return result.is_planned
    except:
        return False

def main():
    rospy.init_node('scan_task_space_limits')
    rospy.wait_for_service('/goal_task_space_path')
    global move_service
    move_service = rospy.ServiceProxy('/goal_task_space_path', SetKinematicsPose)

    print("🔍 Pokrećem ispitivanje radnog prostora manipulatora...")

    # Definiraj testne granice
    x_range = [round(x * 0.01, 2) for x in range(-30, 31, 5)]  # -0.30 do +0.30
    y_range = [round(y * 0.01, 2) for y in range(0, 21, 2)]    # 0.00 do +0.20
    z_range = [round(z * 0.01, 2) for z in range(5, 31, 5)]    # 0.05 do +0.30

    csv_filename = 'manipulator_task_space_scan.csv'
    with open(csv_filename, mode='w', newline='') as file:
        writer = csv.writer(file)
        writer.writerow(['x (m)', 'y (m)', 'z (m)', 'reachable'])

        for x in x_range:
            for y in y_range:
                for z in z_range:
                    reachable = test_pose(x, y, z)
                    status = "✅" if reachable else "❌"
                    print(f"{status} x={x:.2f}, y={y:.2f}, z={z:.2f}")
                    writer.writerow([x, y, z, reachable])

    print(f"\n📄 Podaci spremljeni u: {csv_filename}")

if __name__ == '__main__':
    main()