Not position but Orientation for IK solver

I tested your library, pretty decent by selecting active joints. However your library minimize its IK solver error by position not by its Orientation. In other words it solves for the Position of the Homo. Transformation Matrix and after that it checks orientation.

How I can solve for orientation as priority?

As I see in the code:

    def optimize_target(x):
        # y = np.append(starting_nodes_angles[:chain.first_active_joint], x)
        y = chain.active_to_full(x, starting_nodes_angles)
        squared_distance = np.linalg.norm(chain.forward_kinematics(y)[:3, -1] - target)
        return squared_distance

This section is specified for target which is position. Any suggestion for Orientation?

[AhmetMericOzcan]

Hello for all faces same problem as me, I found the solution by changing optimization function for orientatition priority. I changed he code as follows:

    # Only get the orientation
    target = target_frame[:-1, 0:3]

    if starting_nodes_angles is None:
        raise ValueError("starting_nodes_angles must be specified")

    # Compute orientation difference
    def optimize_target(x):
        # y = np.append(starting_nodes_angles[:chain.first_active_joint], x)
        y = chain.active_to_full(x, starting_nodes_angles)
        squared_distance = np.linalg.norm(chain.forward_kinematics(y)[:-1, 0:3] - target)
        return squared_distance

With these lines you can use it for orientation too. In this code I cut the rotation matrix from HM and I optimized the code through the difference of target rotation and current rotation. I believe this is how I did it at least 😃

It worked now my local library uses this orientation matrix but not the position. Perfect! Cheers

[AhmetMericOzcan]

Ok mate, Did you understand the logic behind my code? how Itry to decrease the error for orientation? You should do the same for position amd orientation!! Mate check how I cut the HT matrix. I cut it from only rotational Matrix in other words 3×3 Matrix. other 3×1 matrix is position. If you cut the target as 3×4 matrix with out the last line, you can make the error function for your orientation and position together. It is simple try it yourself maybe it xan solve yoyr problem.

More over even if doesnt, try to make the error function depended to the position and orientation together. I believe it can solve your problems. Just try a little