Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

The inverse_kinematics function tells us if the pose is reachable, it should be easier to get that info #480

Open
RemiFabre opened this issue Jan 21, 2025 · 0 comments
Assignees
Labels
enhancement New feature or request

Comments

@RemiFabre
Copy link
Member

Currenlty the function raises ValueErrors for several cases:

raise ValueError(f"No solution found for the given target ({target})!")

But how can a user programmatically know if the pose is reachable or not? This is an important feature.

One way it could be done is to create Exception types:

class InvalidInputError(ValueError):
    """Raised when input arguments are invalid."""

class IKUnreachableError(ValueError):
    """Raised when the target pose is unreachable."""

And change a bit the function:

def inverse_kinematics(
        self,
        target: npt.NDArray[np.float64],
        q0: Optional[List[float]] = None,
        degrees: bool = True,
    ) -> List[float]:
    """Compute a joint configuration to reach a specified target pose for the arm end-effector."""
    if target.shape != (4, 4):
        raise InvalidInputError("Target shape should be (4, 4) (got {target.shape} instead)!")

    if q0 is not None and (len(q0) != 7):
        raise InvalidInputError(f"q0 should be length 7 (got {len(q0)} instead)!")

    if isinstance(q0, np.ndarray) and len(q0.shape) > 1:
        raise InvalidInputError("Vectorized kinematics not supported!")

    req_params = {
        "target": ArmEndEffector(
            pose=Matrix4x4(data=target.flatten().tolist()),
        ),
        "id": self._part_id,
    }

    if q0 is not None:
        req_params["q0"] = list_to_arm_position(q0, degrees)
    else:
        present_joints_positions = [
            joint.present_position for orbita in self._actuators.values() for joint in orbita._joints.values()
        ]
        req_params["q0"] = list_to_arm_position(present_joints_positions, degrees)

    req = ArmIKRequest(**req_params)
    resp = self._stub.ComputeArmIK(req)

    if not resp.success:
        raise IKUnreachableError(f"No solution found for the given target ({target})!")

    answer: List[float] = arm_position_to_list(resp.arm_position, degrees)
    return answer

And then the user could write something like this:

try:
    joint_angles = arm.inverse_kinematics(target_pose, q0=initial_guess, degrees=True)
    print("Pose is reachable. Joint angles:", joint_angles)
except InvalidInputError as e:
    print("Invalid input error:", e)
except IKUnreachableError as e:
    print("Pose is unreachable:", e)
except Exception as e:
    print("Unexpected error:", e)

Or, if this is too verbose, the function could always return two things: joints[7 floats], is_reachable[1 bool]

And we could put this as a "good practice" here:
https://pollen-robotics.github.io/reachy2-docs/developing-with-reachy-2/basics/4-use-arm-kinematics/#inverse-kinematics

Thoughts @glannuzel ?

@RemiFabre RemiFabre self-assigned this Jan 21, 2025
@RemiFabre RemiFabre added the enhancement New feature or request label Jan 21, 2025
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
enhancement New feature or request
Projects
None yet
Development

No branches or pull requests

1 participant