diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index fbabac8..3a2a1e0 100644 --- a/kine_ctrl/main.py +++ b/kine_ctrl/main.py @@ -22,12 +22,12 @@ tools_in_ee = { } # joint limit -ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi -lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi +# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi +# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi -# ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi -# lb = -ub +ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi +lb = -ub tool_name = "scissor" @@ -48,6 +48,17 @@ def main(): robot_kine_rm.add_tool_frames(tools_in_ee) robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True) + # ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532], + # initial_guess=[0.1] * 7, tool="no_tool") + # + # print(f'ret_rm = {ret_rm}, q = {q}') + # pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool") + # print(f'pose = {pose}') + # + # time.sleep(5) + + + # -------------- for comparison ---------------- @@ -59,7 +70,7 @@ def main(): solve_sum = 0 - for i in range(10): + for i in range(1000): print(f'\n-------------- in i = {i} ----------------') joint_rand = np.random.uniform(ub, lb) print(f'the predefined joints are {joint_rand}') @@ -83,27 +94,27 @@ def main(): if ret_qp == 0: fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2) - print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}') + print(f'---- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}') if d_p_ik < 0.01: result[0][1] += 1 - robot_mjk.send_command(q) - robot_mjk.wait_until_reached() - robot_mjk.print_state() + # robot_mjk.send_command(q) + # robot_mjk.wait_until_reached() + # robot_mjk.print_state() else: fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name) d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2) - print(f'-- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}') + print(f'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}') ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name) if ret_rm == 0: fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name) d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2) - print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}') + print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}') if d_p_ik < 0.01: result[1][1] += 1 else: - print(f'== fail in the rm ik, ret = {ret_rm}, q = {q}') + print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}') if ret_qp == 0 or ret_rm == 0: solve_sum += 1 diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py index 34fa4cb..54853b4 100644 --- a/kine_ctrl/rm75_kine_rm.py +++ b/kine_ctrl/rm75_kine_rm.py @@ -7,7 +7,7 @@ class rm75_kine_api(): def __init__(self): # ---------- rm75 official algorithm ----------- print(f'------- the realman official kinematic initialising -------') - arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm + arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_75 Robotic arm force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version # Initialize the robotic arm model and sensor type in the algorithm self.robot_kine_rm = Algo(arm_model, force_type) @@ -120,14 +120,36 @@ class rm75_kine_api(): self.work_name = work self.cfg_work_frame(work) - target = target_position + target_rpy + target = list(target_position) + list(target_rpy) if initial_guess is not None: q_ref = [ 180/math.pi * ig for ig in initial_guess ] else: q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) + # print(f'the arm angle is ret = {ret}, and phi = {phi}') params = rm_inverse_kinematics_params_t(q_ref, target, 1) ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) - return ret, [ q/180*math.pi for q in q_out] \ No newline at end of file + pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1) + pose_dis = cal_pose_deviation(pose_fk, target) + + # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}') + # + # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n') + if ret < 0: + return ret, [ q/180*math.pi for q in q_out] + elif pose_dis < 0.01: + return ret, [ q/180*math.pi for q in q_out] + else: + return -10, [ q/180*math.pi for q in q_out] + +def cal_pose_deviation(pose1, pose2): + d_fk_p1 = np.array(pose1) - np.array(pose2) + for j in [3, 4, 5]: + while d_fk_p1[j] > math.pi: + d_fk_p1[j] -= 2 * math.pi + while d_fk_p1[j] < -math.pi: + d_fk_p1[j] += 2 * math.pi + d_fk = np.linalg.norm(d_fk_p1) + return d_fk diff --git a/kine_ctrl/workspace_comfortable/workspace_cal.py b/kine_ctrl/workspace_comfortable/workspace_cal.py index 39f6be0..cd7f99c 100644 --- a/kine_ctrl/workspace_comfortable/workspace_cal.py +++ b/kine_ctrl/workspace_comfortable/workspace_cal.py @@ -100,11 +100,11 @@ JOINT_NAMES = [ # Cartesian workspace grid, in meters. # Adjust according to your robot placement and task. -X_RANGE = (-0.25, 0.6) -Y_RANGE = (-0.25, 0.6) -Z_RANGE = (0.1, 0.6) +X_RANGE = (-0.6, 0.6) +Y_RANGE = (-0.6, 0.6) +Z_RANGE = (0.0, 0.8) -GRID_RESOLUTION = 0.1 # 5 cm. Use 0.02 for finer but slower. +GRID_RESOLUTION = 0.025 # 5 cm. Use 0.02 for finer but slower. # Comfort thresholds MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits @@ -126,7 +126,7 @@ JACOBIAN_EPS = 1e-5 # 2. TASK ORIENTATION SAMPLING # ============================================================ -def make_task_orientations(num_orientations=60, seed=1): +def make_task_orientations(num_orientations=200, seed=1): """ Random orientation sampling using RM's Euler convention: @@ -229,6 +229,8 @@ def solve_ik_user(target_position, target_rotation): ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name) # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}') if ret_rm == 0: + pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name) + # print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}') return q @@ -548,19 +550,20 @@ def evaluate_workspace(): attempted += 1 # print(f"\n - target point: {point}, target orientation: {rpy}") - + rpy = [1.2022060487764064, -1.0097962261845583, -0.6518417572686532] ik_result = solve_ik_user(point, rpy) + + # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}') candidate_solutions = normalize_ik_solutions(ik_result) - - - if len(candidate_solutions) == 0: continue evaluated_solutions = [] for q in candidate_solutions: + # pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name) + # print(f'the fk of q is {pose}\n') metrics = evaluate_single_solution(robot, q, lower, upper) # print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}') if metrics is not None: