OpenCV Hand-Eye Calibration

Using the OpenCV to execute the Hand-Eye calibraiton is very efficent and comes with good results. Their method implements the solutions to the AX = XB equation. The current implementation uses calibrateHandEye().

There are a couple different methods that can be used to solve the issue, the choices of methods can be found here.

The functions expects the rotation matrices from R_gripper2base and the translation part from the homogenous matrix t_gripper2base, as well as the corresponding parts for the target2cam.

NOTE: Current implementation does not allow fixed camera when doing the hand-eye calibraiton but one could put an ArUco marker in the middle of the gripper and move the gripper around the camera's view frame. This would require an entire new skill or major changes to the current implementation.

Input elements:

• Camera - Camera to do the calibration for.
• Arm - Arm connected with the camera.
• Object - Reference Point used when generating poses, also the target.
• Radius - Radius of sphere when generating poses.
• MaxDistance - Limits the maximum distance of the generated poses from the base of the `Arm`.
• MinDistance - Minimum distance from the base for the generated poses.
• TheesholdPos - How accurate the go_to_linear() functions needs to be before saying that it reached its destination.
• Number of Poses - Number of poses for the hand-eye calibration with a minimum of 2.

Required world model relations:

• Arm skiros:hasA EELink
• Camera skiros:hasA View Frame

The Arm need the the following properties set in the world model:

• skiros:LinkedToFrameId
  • skiros:MotionExe
  • skiros:MoveItGroup (when using MoveIt implementation)
  • skiros:MoveItReferenceFrame (when using MoveIt implementation)
  • skiros:MoveItTCPLink (when using MoveIt implementation)

The EELink (should be renamed to EndEffectorLink) is defined as:

scalable:ArmEELink-888 a scalable:ArmEELink,
      owl:NamedIndividual ;
  rdfs:label "Arm End Effector Link" ;
  skiros:BaseFrameId "cora:Robot-1"^^xsd:string ;
  skiros:ContainerState "Empty"^^xsd:string ;
  skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ;
  skiros:FrameId "ur5e_ee_link"^^xsd:string ;
  skiros:FrontAxis "x"^^xsd:string;
  skiros:LinkedToFrameId "ur5e_ee_link"^^xsd:string ;
  skiros:Orientation...
  skiros:Position...
  skiros:PublishTf true;
  skiros:Template "scalable:arm_end_effector"^^xsd:string ;
  skiros:TfTimeStamp "282.412"^^xsd:float ;
  skiros:at skiros:BlockGraspPose-1 ;
  skiros:hasA scalable:WsgGripper-3 ;
  skiros:contain skiros:DepthCamera-48,
      skiros:TransformationPose-887 ;.

An Object to be observed using ArUco Marker should be defined as:

  skiros:BigBox-143 a skiros:Product,
      owl:NamedIndividual ;
  rdfs:label "BlockWithMarker42" ;
  skiros:BaseFrameId "skiros:DepthCamera-48"^^xsd:string ;
  skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ;
  skiros:FrameId "skiros:BigBox-143"^^xsd:string ;
  skiros:OrientationW "1.0"^^xsd:float ;
  skiros:OrientationX "0.0"^^xsd:float ;
  skiros:OrientationY "0.0"^^xsd:float ;
  skiros:OrientationZ "0.0"^^xsd:float ;
  skiros:PositionX "0.0"^^xsd:float ;
  skiros:PositionY "0.0"^^xsd:float ;
  skiros:PositionZ "0.0"^^xsd:float ;
  skiros:SizeX "0.325"^^xsd:float ;
  skiros:SizeY "0.324"^^xsd:float ;
  skiros:SizeZ "0.236"^^xsd:float ;
  skiros:PublishTf true ;
  skiros:Template "skiros:Product"^^xsd:string ;
  skiros:TfTimeStamp "1639494837.17"^^xsd:float ;
  skiros:at skiros:Location-3 ;
  skiros:hasA scalable:Marker-422-7 .

and its corresponding Marker:

  scalable:Marker-422-7 a scalable:Marker,
owl:NamedIndividual ;
rdfs:label "ArucoMarker0"^^xsd:string ;
skiros:BaseFrameId "skiros:BigBox-143"^^xsd:string ;
  skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ;
  skiros:FrameId "skiros:Marker-422-7"^^xsd:string ;
skiros:OrientationW "0.0"^^xsd:float ;
skiros:OrientationX "0.7071068"^^xsd:float ;
skiros:OrientationY "0.7071068"^^xsd:float ;
skiros:OrientationZ "0.0"^^xsd:float ;
skiros:PositionX "-0.087"^^xsd:float ;
skiros:PositionY "-0.087"^^xsd:float ;
skiros:PositionZ "0.0"^^xsd:float ;
  skiros:PublishTf true ;
skiros:Size "0.174"^^xsd:float ;
skiros:Value "422"^^xsd:int ;
scalable:ArUcoSize "7x7"^^xsd:string .

A Camera skiros:hasA CalibrationParameters which also needs to be modified when getting the data using the aau_spatial_reasonor.py, these are defined as:

scalable:CalibrationParameters-1 a     scalable:CalibrationParameters,
      owl:NamedIndividual ;
  rdfs:label "real_sense_calibration" ;
  scalable:FocalLengthX "607.91"^^xsd:float ;
  scalable:FocalLengthY "608.40"^^xsd:float ;
  scalable:PixelCenterX "435.50"^^xsd:float ;
  scalable:PixelCenterY "225.05"^^xsd:float ;
  scalable:Distortionk1 "0.1203"^^xsd:float ;
  scalable:Distortionk2 "-0.3019"^^xsd:float ;
  scalable:Distortionp1 "0.0002"^^xsd:float ;
  scalable:Distortionp2 "-0.0044"^^xsd:float ;
  scalable:Distortionk3 "0.1847"^^xsd:float ; .
  

Required skills needed by the SkillBase

• ''pose_estimation_aruco''
• ''copy_tf''
• wait (Already exists in system)
• ''random_point_on_sphere_move''
• ''wait_for_velocity''
• switch_controller (Already exists in system)
• ''ee_camera_pose_publisher''
• ''hand_eye_calculation_opencv''

The complete skill can be found here.

• Merge all relevant code to master branches, including heron_launch, and move relevant skills to correct packages as in vision_skills or skills_sandbox.
• Implement so that fixed camera can be solved.