You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
raiseValueError(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]
Currenlty the function raises ValueErrors for several cases:
reachy2-sdk/src/reachy2_sdk/parts/arm.py
Line 366 in 10432a3
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:
And change a bit the function:
And then the user could write something like this:
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 ?
The text was updated successfully, but these errors were encountered: