├── hand_eye_flexbe_states ├── hand_eye_flexbe_states │ ├── __init__.py │ ├── get_calib_pose.py │ ├── move_charuco_center.py │ ├── automatic_move_robot.py │ ├── move_robot_manually.py │ ├── pose_plan.py │ ├── joints_plan.py │ ├── execute_traj.py │ ├── initial_pose.py │ ├── get_ar_marker.py │ ├── moveit_hand_eye_excute.py │ ├── find_charuco.py │ ├── moveit_plan_excute.py │ ├── take_picture.py │ ├── obj_trans_to_arm.py │ ├── charuco_camera_calibration.py │ ├── compute_calib.py │ └── generate_hand_eye_point.py ├── resource │ └── hand_eye_flexbe_states ├── setup.cfg ├── CMakeLists.txt ├── package.xml └── setup.py ├── hand_eye_flexbe_behaviors ├── hand_eye_flexbe_behaviors │ ├── __init__.py │ ├── camera_calibration_sm.py │ ├── manual_hand_eye_calibration_sm.py │ ├── verify_calibraion_sm.py │ ├── hand_eye_calibration_sm.py │ └── automatic_hand_eye_calibration_sm.py ├── resource │ └── hand_eye_flexbe_behaviors ├── config │ └── example.yaml ├── setup.cfg ├── CMakeLists.txt ├── manifest │ ├── hand_eye_calibration.xml │ ├── verify_calibraion.xml │ ├── manual_hand_eye_calibration.xml │ ├── camera_calibration.xml │ └── automatic_hand_eye_calibration.xml ├── package.xml └── setup.py ├── doc └── resource │ ├── flexbe.png │ ├── select_behavior.png │ ├── maunal_hand_eye_cali.png │ ├── auto_hand_eye_calib_execute.png │ └── camera_calibration_execution.png ├── .gitmodules ├── LICENSE └── README.md /hand_eye_flexbe_states/hand_eye_flexbe_states/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/resource/hand_eye_flexbe_states: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/hand_eye_flexbe_behaviors/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/resource/hand_eye_flexbe_behaviors: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/config/example.yaml: -------------------------------------------------------------------------------- 1 | # You can use this folder to place general config files 2 | -------------------------------------------------------------------------------- /doc/resource/flexbe.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tku-iarc/hand-eye-calibration_ROS2/HEAD/doc/resource/flexbe.png -------------------------------------------------------------------------------- /doc/resource/select_behavior.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tku-iarc/hand-eye-calibration_ROS2/HEAD/doc/resource/select_behavior.png -------------------------------------------------------------------------------- /doc/resource/maunal_hand_eye_cali.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tku-iarc/hand-eye-calibration_ROS2/HEAD/doc/resource/maunal_hand_eye_cali.png -------------------------------------------------------------------------------- /doc/resource/auto_hand_eye_calib_execute.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tku-iarc/hand-eye-calibration_ROS2/HEAD/doc/resource/auto_hand_eye_calib_execute.png -------------------------------------------------------------------------------- /doc/resource/camera_calibration_execution.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tku-iarc/hand-eye-calibration_ROS2/HEAD/doc/resource/camera_calibration_execution.png -------------------------------------------------------------------------------- /hand_eye_flexbe_states/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/hand_eye_flexbe_states 3 | [install] 4 | install-scripts=$base/lib/hand_eye_flexbe_states 5 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/hand_eye_flexbe_behaviors 3 | [install] 4 | install-scripts=$base/lib/hand_eye_flexbe_behaviors 5 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "charuco_detector_ROS2"] 2 | path = charuco_detector_ROS2 3 | url = https://github.com/errrr0501/charuco_detector_ROS2.git 4 | [submodule "vision_visp"] 5 | path = vision_visp 6 | url = https://github.com/tku-iarc/vision_visp.git 7 | branch = ros2 8 | [submodule "flexbe_behavior_engine"] 9 | path = flexbe_behavior_engine 10 | url = https://github.com/FlexBE/flexbe_behavior_engine.git 11 | branch = humble-devel 12 | [submodule "flexbe_app"] 13 | path = flexbe_app 14 | url = https://github.com/FlexBE/flexbe_app.git 15 | branch = humble-devel 16 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/get_calib_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from flexbe_core import EventState, Logger 4 | from math import radians 5 | 6 | class GetCalibPoseState(EventState): 7 | """ 8 | Output a fixed pose to move. 9 | 10 | <= done Pose has been published. 11 | <= fail Task fail and finished 12 | 13 | """ 14 | 15 | def __init__(self): 16 | """Constructor""" 17 | super(GetCalibPoseState, self).__init__(outcomes=['done', 'fail']) 18 | 19 | def execute(self, userdata): 20 | if True: 21 | return 'done' 22 | else: 23 | return 'fail' 24 | 25 | def on_enter(self, userdata): 26 | pass -------------------------------------------------------------------------------- /hand_eye_flexbe_states/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(hand_eye_flexbe_states) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(ament_cmake_python REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(rclpy REQUIRED) 16 | 17 | install(DIRECTORY 18 | DESTINATION lib/${PROJECT_NAME} 19 | ) 20 | 21 | # Install Python modules 22 | ament_python_install_package(${PROJECT_NAME}) 23 | 24 | ament_package() 25 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/move_charuco_center.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from flexbe_core import EventState, Logger 4 | from math import radians 5 | import random 6 | 7 | class MoveCharucoCenterState(EventState): 8 | """ 9 | Output a fixed pose to move. 10 | 11 | <= done Pose has been published. 12 | <= fail Task fail and finished 13 | 14 | """ 15 | 16 | def __init__(self): 17 | """Constructor""" 18 | super(MoveCharucoCenterState, self).__init__(outcomes=['done', 'fail']) 19 | 20 | def execute(self, userdata): 21 | if True: 22 | return 'done' 23 | else: 24 | return 'fail' 25 | 26 | def on_enter(self, userdata): 27 | pass -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(hand_eye_flexbe_behaviors) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake REQUIRED) 13 | find_package(ament_cmake_python REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(rclpy REQUIRED) 16 | 17 | install(DIRECTORY 18 | manifest 19 | DESTINATION lib/${PROJECT_NAME} 20 | ) 21 | 22 | install(DIRECTORY 23 | config 24 | DESTINATION lib/${PROJECT_NAME} 25 | ) 26 | 27 | # Install Python modules 28 | ament_python_install_package(${PROJECT_NAME}) 29 | 30 | ament_package() 31 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/manifest/hand_eye_calibration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | v0 7 | Andy Chien 8 | Tue Nov 09 2021 9 | 10 | Used to calibrate the camera on hand or in hand 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | hand_eye_flexbe_behaviors 7 | 1.0.0 8 | 9 | hand_eye_flexbe_behaviors references all implemented behaviors. 10 | 11 | TODO 12 | BSD 13 | 14 | rclcpp 15 | rclpy 16 | 17 | ament_copyright 18 | ament_flake8 19 | ament_pep257 20 | python3-pytest 21 | 22 | ament_cmake 23 | ament_cmake_python 24 | 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | hand_eye_flexbe_states 7 | 1.0.0 8 | 9 | hand_eye_flexbe_states provides a collection of custom states. 10 | Feel free to add new states. 11 | 12 | TODO 13 | BSD 14 | 15 | rclcpp 16 | rclpy 17 | flexbe_core 18 | 19 | ament_copyright 20 | ament_flake8 21 | ament_pep257 22 | python3-pytest 23 | 24 | ament_cmake 25 | ament_cmake_python 26 | 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Chien shao-yu 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/manifest/verify_calibraion.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | Luis 8 | Thu Nov 17 2022 9 | 10 | verify_calibraion 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | from setuptools import setup 4 | 5 | package_name = 'hand_eye_flexbe_behaviors' 6 | 7 | setup( 8 | name=package_name, 9 | version='1.3.1', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | ], 16 | install_requires=['setuptools'], 17 | zip_safe=True, 18 | maintainer='Luis', 19 | maintainer_email='errrr0501done@gmail.com', 20 | description='TODO: Package description', 21 | license='TODO: License declaration', 22 | tests_require=['pytest'], 23 | entry_points={ 24 | 'console_scripts': [ 25 | 'automatic_hand_eye_calibration_sm = hand_eye_flexbe_behaviors.automatic_hand_eye_calibration_sm', 26 | 'camera_calibration_sm = hand_eye_flexbe_behaviors.camera_calibration_sm', 27 | 'hand_eye_calibration_sm = hand_eye_flexbe_behaviors.hand_eye_calibration_sm', 28 | 'manual_hand_eye_calibration_sm = hand_eye_flexbe_behaviors.manual_hand_eye_calibration_sm', 29 | 'verify_calibraion_sm = hand_eye_flexbe_behaviors.verify_calibraion_sm', 30 | ], 31 | }, 32 | ) -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/automatic_move_robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from flexbe_core import EventState 3 | 4 | 5 | class AutomaticMoveRobot(EventState): 6 | ''' 7 | Implements a state that can be used to wait on timed process. 8 | 9 | -- wait_time float Amount of time to wait in seconds. 10 | -- pose_num int Number of pose to calibrate 11 | 12 | #> result_compute bool Ready to compute the result 13 | 14 | <= done Indicates that the wait time has elapsed. 15 | ''' 16 | 17 | def __init__(self, wait_time, pose_num): 18 | super(AutomaticMoveRobot, self).__init__(outcomes=['done'], output_keys=['result_compute']) 19 | self._wait = wait_time 20 | self._pose_num = pose_num 21 | self._pose_count = 0 22 | 23 | def execute(self, userdata): 24 | elapsed = AutomaticMoveRobot._node.get_clock().now() - self._start_time 25 | userdata.result_compute = self._pose_count >= self._pose_num 26 | if elapsed.to_sec() > self._wait: 27 | return 'done' 28 | 29 | def on_enter(self, userdata): 30 | '''Upon entering the state, save the current time and start waiting.''' 31 | self._start_time = AutomaticMoveRobot._node.get_clock().now() 32 | self._pose_count += 1 -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/manifest/manual_hand_eye_calibration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | v0 7 | Andy Chien 8 | Sun Nov 14 2021 9 | 10 | Execute hand eye calibration by manual 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/move_robot_manually.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from flexbe_core import EventState, Logger 3 | from rclpy.clock import Clock, ClockType 4 | 5 | 6 | 7 | class MoveRobotManuallyState(EventState): 8 | ''' 9 | Implements a state that can be used to wait on timed process. 10 | 11 | -- wait_time float Amount of time to wait in seconds. 12 | -- pose_num int Number of pose to calibrate 13 | 14 | #> result_compute bool Ready to compute the result 15 | 16 | <= done Indicates that the wait time has elapsed. 17 | ''' 18 | 19 | def __init__(self, wait_time, pose_num): 20 | super(MoveRobotManuallyState, self).__init__(outcomes=['done'], output_keys=['result_compute']) 21 | self._wait = wait_time 22 | self._pose_num = pose_num 23 | self._pose_count = 0 24 | 25 | def execute(self, userdata): 26 | # elapsed = MoveRobotManuallyState._node.get_clock().now().seconds() - self._start_time 27 | elapsed = Clock(clock_type=ClockType.ROS_TIME).now() - self._start_time 28 | userdata.result_compute = self._pose_count >= self._pose_num 29 | if elapsed.nanoseconds / 1000000000 > self._wait: 30 | return 'done' 31 | 32 | def on_enter(self, userdata): 33 | '''Upon entering the state, save the current time and start waiting.''' 34 | self._start_time = Clock(clock_type=ClockType.ROS_TIME).now() 35 | print(self._start_time) 36 | self._pose_count += 1 37 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/manifest/camera_calibration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | Luis 8 | Tue Nov 15 2022 9 | 10 | camera_calibration 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/pose_plan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import enum 3 | import moveit_commander 4 | from moveit_msgs.msg import MoveItErrorCodes 5 | from flexbe_core import EventState, Logger 6 | from flexbe_core.proxy import ProxyActionClient 7 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 8 | 9 | ''' 10 | Created on 15.06.2015 11 | 12 | @author: Philipp Schillinger 13 | ''' 14 | 15 | class PosePlanState(EventState): 16 | ''' 17 | Uses dcma planner to plan or plan and move the specified joints to the target configuration. 18 | 19 | -- group_name string move group name 20 | 21 | <= done Target joint configuration has been planned. 22 | <= failed Failed to find a plan to the given joint configuration. 23 | ''' 24 | 25 | 26 | def __init__(self, group_name): 27 | ''' 28 | Constructor 29 | ''' 30 | super(PosePlanState, self).__init__(outcomes=['failed', 'done'], output_keys=['joint_trajectory', 'target_joints']) 31 | # group_name = "" 32 | self._group_name = group_name 33 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 34 | self._move_group.set_planner_id("RRTConnectkConfigDefault") 35 | self._move_group.set_planning_time(1) 36 | self._result = None 37 | 38 | def execute(self, userdata): 39 | ''' 40 | Execute this state 41 | ''' 42 | if True: 43 | return 'done' 44 | else: 45 | return 'failed' 46 | 47 | def on_enter(self, userdata): 48 | # (success flag : boolean, trajectory message : RobotTrajectory, 49 | # planning time : float, error code : MoveitErrorCodes) 50 | speed = 0.1 51 | self._move_group.set_max_velocity_scaling_factor(speed) 52 | self._move_group.set_max_acceleration_scaling_factor(speed) 53 | 54 | def on_stop(self): 55 | pass 56 | 57 | def on_pause(self): 58 | pass 59 | 60 | def on_resume(self, userdata): 61 | self.on_enter(userdata) 62 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from setuptools import setup 3 | from setuptools import find_packages 4 | 5 | package_name = 'hand_eye_flexbe_states' 6 | 7 | setup( 8 | name=package_name, 9 | version='1.3.1', 10 | packages=find_packages(), 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | ], 16 | install_requires=['setuptools'], 17 | zip_safe=True, 18 | maintainer='Luis', 19 | maintainer_email='errrr0501done@gmail.com', 20 | description='TODO: Package description', 21 | license='TODO: License declaration', 22 | tests_require=['pytest'], 23 | entry_points={ 24 | 'console_scripts': [ 25 | 'automatic_move_robot = hand_eye_flexbe_states.automatic_move_robot', 26 | 'charuco_camera_calibration = hand_eye_flexbe_states.charuco_camera_calibration', 27 | 'compute_calib = hand_eye_flexbe_states.compute_calib', 28 | 'execute_traj = hand_eye_flexbe_states.execute_traj', 29 | 'find_charuco = hand_eye_flexbe_states.find_charuco', 30 | 'generate_hand_eye_point = hand_eye_flexbe_states.generate_hand_eye_point', 31 | 'get_ar_marker = hand_eye_flexbe_states.get_ar_marker', 32 | 'get_calib_pose = hand_eye_flexbe_states.get_calib_pose', 33 | 'initial_pose = hand_eye_flexbe_states.initial_pose', 34 | 'joints_plan = hand_eye_flexbe_states.joints_plan', 35 | 'move_charuco_center = hand_eye_flexbe_states.move_charuco_center', 36 | 'moveit_hand_eye_excute = hand_eye_flexbe_states.moveit_hand_eye_excute', 37 | 'moveit_plan_excute = hand_eye_flexbe_states.moveit_plan_excute', 38 | 'move_robot_manually = hand_eye_flexbe_states.move_robot_manually', 39 | 'obj_trans_to_arm = hand_eye_flexbe_states.obj_trans_to_arm', 40 | 'pose_plan = hand_eye_flexbe_states.pose_plan', 41 | 'take_picture = hand_eye_flexbe_states.take_picture', 42 | ], 43 | }, 44 | ) 45 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/manifest/automatic_hand_eye_calibration.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | v0 7 | Luis Tsai 8 | Sun Nov 14 2021 9 | 10 | Execute hand eye calibration by manual 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/joints_plan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import enum 4 | import moveit_commander 5 | from moveit_msgs.msg import MoveItErrorCodes 6 | from flexbe_core import EventState, Logger 7 | from flexbe_core.proxy import ProxyActionClient 8 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 9 | 10 | ''' 11 | Created on 15.06.2015 12 | 13 | @author: Philipp Schillinger 14 | ''' 15 | 16 | class JointsPlan(EventState): 17 | ''' 18 | Uses dcma planner to plan or plan and move the specified joints to the target configuration. 19 | 20 | -- group_name string move group name 21 | 22 | ># joint_config float[] Target configuration of the joints. 23 | Same order as their corresponding names in joint_names. 24 | 25 | #> joint_trajectory JointTrajectory planned or executed trajectory 26 | 27 | <= done Target joint configuration has been planned. 28 | <= failed Failed to find a plan to the given joint configuration. 29 | ''' 30 | 31 | 32 | def __init__(self, group_name): 33 | ''' 34 | Constructor 35 | ''' 36 | super(JointsPlan, self).__init__(outcomes=['failed', 'done'], 37 | input_keys=['joint_config'], 38 | output_keys=['joint_trajectory', 'target_joints']) 39 | # group_name = "" 40 | self._group_name = group_name 41 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 42 | self._move_group.set_planner_id("RRTConnectkConfigDefault") 43 | self._move_group.set_planning_time(1) 44 | self._result = None 45 | 46 | def execute(self, userdata): 47 | ''' 48 | Execute this state 49 | ''' 50 | if len(self._result.joint_trajectory.points) > 0: 51 | userdata.joint_trajectory = self._result 52 | userdata.target_joints = userdata.joint_config 53 | return 'done' 54 | else: 55 | return 'failed' 56 | 57 | def on_enter(self, userdata): 58 | # (success flag : boolean, trajectory message : RobotTrajectory, 59 | # planning time : float, error code : MoveitErrorCodes) 60 | speed = 0.1 61 | self._move_group.set_max_velocity_scaling_factor(speed) 62 | self._move_group.set_max_acceleration_scaling_factor(speed) 63 | self._result = self._move_group.plan(userdata.joint_config) 64 | 65 | def on_stop(self): 66 | pass 67 | 68 | def on_pause(self): 69 | pass 70 | 71 | def on_resume(self, userdata): 72 | self.on_enter(userdata) 73 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/execute_traj.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import moveit_commander 3 | from moveit_msgs.msg import MoveItErrorCodes 4 | from math import pi, radians 5 | from std_msgs.msg import String 6 | from moveit_commander.conversions import pose_to_list 7 | from tf import transformations as tf 8 | 9 | from flexbe_core import EventState, Logger 10 | 11 | from flexbe_core.proxy import ProxyActionClient 12 | 13 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 14 | # from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction, JointTrajectoryControllerState, FollowJointTrajectoryResult 15 | 16 | ''' 17 | Created on 15.06.2015 18 | 19 | @author: Philipp Schillinger 20 | ''' 21 | 22 | class TrajectoryExecuteState(EventState): 23 | ''' 24 | Move robot by planned trajectory. 25 | 26 | -- group_name string move group name 27 | 28 | ># joint_trajectory JointTrajectory planned trajectory 29 | 30 | <= done Robot move done. 31 | <= failed Robot move failed. 32 | <= collision Robot during collision. 33 | ''' 34 | 35 | 36 | def __init__(self, group_name): 37 | ''' 38 | Constructor 39 | ''' 40 | super(TrajectoryExecuteState, self).__init__(outcomes=['done', 'failed', 'collision'], 41 | input_keys=['joint_trajectory', 'target_joints'], 42 | output_keys=['joint_config']) 43 | # group_name = "" 44 | self._group_name = group_name 45 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 46 | self._result = MoveItErrorCodes.FAILURE 47 | 48 | def stop(self): 49 | pass 50 | 51 | def execute(self, userdata): 52 | ''' 53 | Execute this state 54 | ''' 55 | print("") 56 | print("==================================================================") 57 | print(self._result) 58 | print("==================================================================") 59 | print("") 60 | if self._result == MoveItErrorCodes.SUCCESS: 61 | return 'done' 62 | else: 63 | userdata.joint_config = userdata.target_joints 64 | return 'collision' 65 | 66 | # if self._result == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: 67 | # userdata.joint_config = userdata.target_joints 68 | # return 'collision' 69 | # else: 70 | # return 'failed' 71 | 72 | def on_enter(self, userdata): 73 | self._result = self._move_group.execute(userdata.joint_trajectory) 74 | 75 | def on_stop(self): 76 | pass 77 | 78 | def on_pause(self): 79 | pass 80 | 81 | def on_resume(self, userdata): 82 | self.on_enter(userdata) 83 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/initial_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import moveit_commander 5 | from moveit_msgs.msg import MoveItErrorCodes 6 | from math import pi, radians 7 | from std_msgs.msg import String 8 | import geometry_msgs.msg 9 | from moveit_commander.conversions import pose_to_list 10 | from tf import transformations as tf 11 | 12 | from flexbe_core import EventState, Logger 13 | 14 | from flexbe_core.proxy import ProxyActionClient 15 | 16 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 17 | # from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction, JointTrajectoryControllerState, FollowJointTrajectoryResult 18 | 19 | ''' 20 | Created on 15.06.2015 21 | 22 | @author: Philipp Schillinger 23 | ''' 24 | 25 | class InitialPoseState(EventState): 26 | ''' 27 | Move robot by planned trajectory. 28 | 29 | -- group_name string move group name 30 | 31 | ># joint_trajectory JointTrajectory planned trajectory 32 | 33 | <= done Robot move done. 34 | <= failed Robot move failed. 35 | <= collision Robot during collision. 36 | ''' 37 | 38 | 39 | def __init__(self, group_name,reference_frame): 40 | ''' 41 | Constructor 42 | ''' 43 | super(InitialPoseState, self).__init__(outcomes=['done', 'collision']) 44 | # group_name = "" 45 | self._group_name = group_name 46 | self._reference_frame = reference_frame 47 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 48 | self._result = MoveItErrorCodes.FAILURE 49 | self._move_group.set_pose_reference_frame(self._reference_frame) 50 | self._end_effector_link = self._move_group.get_end_effector_link() 51 | self._move_group.set_end_effector_link(self._end_effector_link) 52 | self._move_group.set_max_acceleration_scaling_factor(0.1) 53 | self._move_group.set_max_velocity_scaling_factor(0.1) 54 | 55 | def stop(self): 56 | pass 57 | 58 | def execute(self, userdata): 59 | ''' 60 | Execute this state 61 | ''' 62 | joint_goal= self._move_group.get_current_joint_values() 63 | joint_goal[0] = pi * 0.5 64 | joint_goal[1] = -pi * 0.5 65 | joint_goal[2] = pi * 0.5 66 | joint_goal[3] = -pi * 0.5 67 | joint_goal[4] = -pi * 0.5 68 | joint_goal[5] = -pi * 0.5 69 | input() 70 | self._result = self._move_group.go(joint_goal, wait=True) 71 | self._move_group.stop() 72 | self._move_group.clear_pose_targets() 73 | 74 | 75 | if self._result == MoveItErrorCodes.SUCCESS: 76 | return 'done' 77 | 78 | elif self._result == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: 79 | return 'collision' 80 | 81 | 82 | def on_enter(self, userdata): 83 | pass 84 | 85 | def on_stop(self): 86 | pass 87 | 88 | def on_pause(self): 89 | pass 90 | 91 | def on_resume(self, userdata): 92 | self.on_enter(userdata) 93 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/hand_eye_flexbe_behaviors/camera_calibration_sm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | ########################################################### 4 | # WARNING: Generated code! # 5 | # ************************** # 6 | # Manual changes may get lost if file is generated again. # 7 | # Only code inside the [MANUAL] tags will be kept. # 8 | ########################################################### 9 | 10 | from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger 11 | from hand_eye_flexbe_states.charuco_camera_calibration import CharucoCameraCalibrationState 12 | from hand_eye_flexbe_states.take_picture import TakePictureState 13 | # Additional imports can be added inside the following tags 14 | # [MANUAL_IMPORT] 15 | 16 | # [/MANUAL_IMPORT] 17 | 18 | 19 | ''' 20 | Created on Tue Nov 15 2022 21 | @author: Luis 22 | ''' 23 | class camera_calibrationSM(Behavior): 24 | ''' 25 | camera_calibration 26 | ''' 27 | 28 | 29 | def __init__(self, node): 30 | super(camera_calibrationSM, self).__init__() 31 | self.name = 'camera_calibration' 32 | self.node = node 33 | 34 | # parameters of this behavior 35 | CharucoCameraCalibrationState.initialize_ros(node) 36 | TakePictureState.initialize_ros(node) 37 | OperatableStateMachine.initialize_ros(node) 38 | Logger.initialize(node) 39 | self.add_parameter('pic_num', 30) 40 | self.add_parameter('square_size', 0.0200) 41 | self.add_parameter('marker_size', 0.0150) 42 | self.add_parameter('col_count', 10) 43 | self.add_parameter('row_count', 14) 44 | self.add_parameter('camera_type', 'realsense') 45 | self.add_parameter('save_file_name', 'camera_calibration.ini') 46 | 47 | # references to used behaviors 48 | 49 | # Additional initialization code can be added inside the following tags 50 | # [MANUAL_INIT] 51 | 52 | # [/MANUAL_INIT] 53 | 54 | # Behavior comments: 55 | 56 | 57 | 58 | def create(self): 59 | # x:570 y:120, x:130 y:365 60 | _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) 61 | 62 | # Additional creation code can be added inside the following tags 63 | # [MANUAL_CREATE] 64 | 65 | # [/MANUAL_CREATE] 66 | 67 | 68 | with _state_machine: 69 | # x:149 y:88 70 | OperatableStateMachine.add('take_camera_cali_pic', 71 | TakePictureState(pic_num=self.pic_num, camera_type=self.camera_type), 72 | transitions={'done': 'camera_calibration', 'failed': 'failed'}, 73 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}) 74 | 75 | # x:348 y:87 76 | OperatableStateMachine.add('camera_calibration', 77 | CharucoCameraCalibrationState(square_size=self.square_size, marker_size=self.marker_size, col_count=self.col_count, row_count=self.row_count, save_file_name=self.save_file_name), 78 | transitions={'done': 'finished', 'failed': 'failed'}, 79 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}) 80 | 81 | 82 | return _state_machine 83 | 84 | 85 | # Private functions can be added inside the following tags 86 | # [MANUAL_FUNC] 87 | 88 | # [/MANUAL_FUNC] 89 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/get_ar_marker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import tf 3 | from flexbe_core import EventState, Logger 4 | from geometry_msgs.msg import Transform 5 | from visp_hand2eye_calibration.msg import TransformArray 6 | import time 7 | 8 | class GetArMarkerState(EventState): 9 | """ 10 | Output a fixed pose to move. 11 | 12 | <= done Charuco pose has been received. 13 | <= go_compute Ready to compute the result. 14 | 15 | """ 16 | 17 | def __init__(self, eye_in_hand_mode): 18 | """Constructor""" 19 | super(GetArMarkerState, self).__init__(outcomes=['done', 'failed'], 20 | output_keys=['camera_h_charuco']) 21 | # self.base_link = base_link 22 | # self.tip_link = tip_link 23 | self.eye_in_hand_mode = eye_in_hand_mode 24 | self.tf_listener = tf.TransformListener() 25 | # self.base_h_tool = TransformArray() 26 | self.camera_h_charuco = TransformArray() 27 | # self.base_h_tool.header.frame_id = self.base_link 28 | self.camera_h_charuco.header.frame_id = 'calib_camera' 29 | self.enter_time = GetArMarkerState._node.get_clock().now() 30 | 31 | def execute(self, userdata): 32 | time.sleep(1) 33 | if (GetArMarkerState._node.get_clock().now() - self.enter_time).to_sec() > 2: 34 | Logger.logwarn('Can not get charuco board pose, abandon this position') 35 | return 'failed' 36 | 37 | 38 | try: 39 | (camera_trans_charuco, camera_rot_charuco) = self.tf_listener.lookupTransform('/calib_camera', '/calib_charuco', self.get_clock().now()) 40 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 41 | Logger.logwarn('lookupTransform for charuco failed!') 42 | return 43 | 44 | # try: 45 | # (base_trans_tool, base_rot_tool) = self.tf_listener.lookupTransform(self.base_link, self.tip_link, rospy.Time(0)) 46 | # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 47 | # self.get_logger().warn('lookupTransform for robot failed!, ' + self.base_link + ', ' + self.tip_link) 48 | # return 49 | 50 | 51 | trans = Transform() 52 | trans.translation.x = camera_trans_charuco[0] 53 | trans.translation.y = camera_trans_charuco[1] 54 | trans.translation.z = camera_trans_charuco[2] 55 | trans.rotation.x = camera_rot_charuco[0] 56 | trans.rotation.y = camera_rot_charuco[1] 57 | trans.rotation.z = camera_rot_charuco[2] 58 | trans.rotation.w = camera_rot_charuco[3] 59 | self.camera_h_charuco.transforms.append(trans) 60 | print(self.camera_h_charuco.transforms) 61 | # print(self.camera_h_charuco) 62 | # print(self.camera_h_charuco.transforms[0].translation.x) 63 | # print(self.camera_h_charuco.transforms[0].translation.y) 64 | # print(self.camera_h_charuco.transforms[0].translation.z) 65 | print("============================") 66 | 67 | # trans = Transform() 68 | # trans.translation.x = base_trans_tool[0] 69 | # trans.translation.y = base_trans_tool[1] 70 | # trans.translation.z = base_trans_tool[2] 71 | # trans.rotation.x = base_rot_tool[0] 72 | # trans.rotation.y = base_rot_tool[1] 73 | # trans.rotation.z = base_rot_tool[2] 74 | # trans.rotation.w = base_rot_tool[3] 75 | # self.base_h_tool.transforms.append(trans) 76 | # print(self.base_h_tool.transforms) 77 | # print("============================") 78 | userdata.camera_h_charuco = self.camera_h_charuco 79 | return 'done' 80 | 81 | # if userdata.result_compute: 82 | # # userdata.base_h_tool = self.base_h_tool 83 | # userdata.camera_h_charuco = self.camera_h_charuco 84 | # return 'done' 85 | # else: 86 | # return 'done' 87 | 88 | def on_enter(self, userdata): 89 | self.enter_time = GetArMarkerState._node.get_clock().now() 90 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/hand_eye_flexbe_behaviors/manual_hand_eye_calibration_sm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | ########################################################### 4 | # WARNING: Generated code! # 5 | # ************************** # 6 | # Manual changes may get lost if file is generated again. # 7 | # Only code inside the [MANUAL] tags will be kept. # 8 | ########################################################### 9 | 10 | from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger 11 | from hand_eye_flexbe_states.compute_calib import ComputeCalibState 12 | from hand_eye_flexbe_states.find_charuco import FindCharucoState 13 | from hand_eye_flexbe_states.move_robot_manually import MoveRobotManuallyState 14 | # Additional imports can be added inside the following tags 15 | # [MANUAL_IMPORT] 16 | 17 | # [/MANUAL_IMPORT] 18 | 19 | 20 | ''' 21 | Created on Sun Nov 14 2021 22 | @author: Andy Chien 23 | ''' 24 | class ManualHandEyeCalibrationSM(Behavior): 25 | ''' 26 | Execute hand eye calibration by manual 27 | ''' 28 | 29 | 30 | def __init__(self, node): 31 | super(ManualHandEyeCalibrationSM, self).__init__() 32 | self.name = 'Manual Hand Eye Calibration' 33 | 34 | # parameters of this behavior 35 | self.add_parameter('eye_in_hand', False) 36 | self.add_parameter('calib_pose_num', 10) 37 | self.add_parameter('base_link', 'base_link') 38 | self.add_parameter('tip_link', 'tool0') 39 | self.add_parameter('calibration_file_name', 'hand_eye_calibration.ini') 40 | self.add_parameter('customize_file', False) 41 | 42 | # references to used behaviors 43 | OperatableStateMachine.initialize_ros(node) 44 | ConcurrencyContainer.initialize_ros(node) 45 | PriorityContainer.initialize_ros(node) 46 | Logger.initialize(node) 47 | ComputeCalibState.initialize_ros(node) 48 | FindCharucoState.initialize_ros(node) 49 | MoveRobotManuallyState.initialize_ros(node) 50 | 51 | # Additional initialization code can be added inside the following tags 52 | # [MANUAL_INIT] 53 | 54 | # [/MANUAL_INIT] 55 | 56 | # Behavior comments: 57 | 58 | 59 | 60 | def create(self): 61 | # x:308 y:369, x:130 y:365 62 | _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) 63 | 64 | # Additional creation code can be added inside the following tags 65 | # [MANUAL_CREATE] 66 | 67 | # [/MANUAL_CREATE] 68 | 69 | 70 | with _state_machine: 71 | # x:71 y:102 72 | OperatableStateMachine.add('Move_Robot_Manually', 73 | MoveRobotManuallyState(wait_time=2, pose_num=self.calib_pose_num), 74 | transitions={'done': 'Find_Charuco'}, 75 | autonomy={'done': Autonomy.Low}, 76 | remapping={'result_compute': 'result_compute'}) 77 | 78 | # x:301 y:92 79 | OperatableStateMachine.add('Find_Charuco', 80 | FindCharucoState(base_link=self.base_link, tip_link=self.tip_link, eye_in_hand_mode=self.eye_in_hand), 81 | transitions={'done': 'Move_Robot_Manually', 'go_compute': 'Calibration_Computation'}, 82 | autonomy={'done': Autonomy.Off, 'go_compute': Autonomy.Off}, 83 | remapping={'result_compute': 'result_compute', 'base_h_tool': 'base_h_tool', 'camera_h_charuco': 'camera_h_charuco'}) 84 | 85 | # x:302 y:228 86 | OperatableStateMachine.add('Calibration_Computation', 87 | ComputeCalibState(eye_in_hand_mode=self.eye_in_hand, calibration_file_name=self.calibration_file_name, customize_file=self.customize_file), 88 | transitions={'finish': 'finished'}, 89 | autonomy={'finish': Autonomy.Off}, 90 | remapping={'base_h_tool': 'base_h_tool', 'camera_h_charuco': 'camera_h_charuco'}) 91 | 92 | 93 | return _state_machine 94 | 95 | 96 | # Private functions can be added inside the following tags 97 | # [MANUAL_FUNC] 98 | 99 | # [/MANUAL_FUNC] 100 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/moveit_hand_eye_excute.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import moveit_commander 5 | from moveit_msgs.msg import MoveItErrorCodes 6 | from math import pi, radians 7 | from std_msgs.msg import String 8 | import geometry_msgs.msg 9 | from moveit_commander.conversions import pose_to_list 10 | from tf import transformations as tf 11 | 12 | from flexbe_core import EventState, Logger 13 | 14 | from flexbe_core.proxy import ProxyActionClient 15 | 16 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 17 | 18 | 19 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 20 | # from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction, JointTrajectoryControllerState, FollowJointTrajectoryResult 21 | 22 | ''' 23 | Created on 15.06.2015 24 | 25 | @author: Philipp Schillinger 26 | ''' 27 | 28 | class MoveitHandEyeExecuteState(EventState): 29 | ''' 30 | Move robot by planned trajectory. 31 | 32 | -- group_name string move group name 33 | 34 | ># joint_trajectory JointTrajectory planned trajectory 35 | 36 | <= done Robot move done. 37 | <= failed Robot move failed. 38 | <= collision Robot during collision. 39 | ''' 40 | 41 | 42 | def __init__(self, group_name,reference_frame, points_num): 43 | ''' 44 | Constructor 45 | ''' 46 | super(MoveitHandEyeExecuteState, self).__init__(outcomes=['received','done', 'finish_correction', 'collision'], 47 | input_keys=['hand_eye_points','motion_state'], 48 | output_keys=['result_compute']) 49 | # group_name = "" 50 | self._group_name = group_name 51 | self._reference_frame = reference_frame 52 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 53 | self._result = MoveItErrorCodes.FAILURE 54 | self._move_group.set_pose_reference_frame(self._reference_frame) 55 | self._end_effector_link = self._move_group.get_end_effector_link() 56 | self._move_group.set_end_effector_link(self._end_effector_link) 57 | self._move_group.set_max_acceleration_scaling_factor(0.1) 58 | self._move_group.set_max_velocity_scaling_factor(0.1) 59 | self._first_joints= self._move_group.get_current_joint_values() 60 | self.points_num = points_num 61 | self._execute_times = 0 62 | self.plan, self.fraction =0,0 63 | self.centralize = False 64 | self.correct_rotation = False 65 | 66 | def on_start(self): 67 | 68 | pass 69 | 70 | def execute(self, userdata): 71 | ''' 72 | Execute this state 73 | ''' 74 | 75 | print(np.size(userdata.hand_eye_points)) 76 | print(self._execute_times) 77 | self.plan = userdata.hand_eye_points[self._execute_times] 78 | # input() 79 | # excute planing path 80 | self._result = self._move_group.execute(self.plan, wait=True) 81 | 82 | userdata.result_compute = self._execute_times >= self.points_num -1 83 | 84 | 85 | if userdata.result_compute: 86 | return 'done' 87 | 88 | if self._result == MoveItErrorCodes.SUCCESS: 89 | self._execute_times += 1 90 | if not self.centralize or not self.correct_rotation: 91 | self._first_joints= self._move_group.get_current_joint_values() 92 | self._execute_times = 0 93 | if userdata.motion_state == 'centralize': 94 | self.centralize = True 95 | return 'finish_correction' 96 | elif userdata.motion_state == 'correct_rotation': 97 | self.correct_rotation = True 98 | return 'finish_correction' 99 | return 'received' 100 | 101 | elif self._result == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: 102 | return 'collision' 103 | 104 | 105 | def on_enter(self, userdata): 106 | # back to center pose 107 | self._result = self._move_group.go(self._first_joints, wait=True) 108 | self._move_group.stop() 109 | self._move_group.clear_pose_targets() 110 | pass 111 | 112 | def on_stop(self): 113 | pass 114 | 115 | def on_pause(self): 116 | pass 117 | 118 | def on_resume(self, userdata): 119 | # self.on_enter(userdata) 120 | pass 121 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/hand_eye_flexbe_behaviors/verify_calibraion_sm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | ########################################################### 4 | # WARNING: Generated code! # 5 | # ************************** # 6 | # Manual changes may get lost if file is generated again. # 7 | # Only code inside the [MANUAL] tags will be kept. # 8 | ########################################################### 9 | 10 | from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger 11 | from hand_eye_flexbe_states.get_ar_marker import GetArMarkerState 12 | from hand_eye_flexbe_states.initial_pose import InitialPoseState 13 | from hand_eye_flexbe_states.moveit_plan_excute import MoveitPlanExecuteState 14 | from hand_eye_flexbe_states.obj_trans_to_arm import ObjTransToArmState 15 | # Additional imports can be added inside the following tags 16 | # [MANUAL_IMPORT] 17 | 18 | # [/MANUAL_IMPORT] 19 | 20 | 21 | ''' 22 | Created on Thu Nov 17 2022 23 | @author: Luis 24 | ''' 25 | class verify_calibraionSM(Behavior): 26 | ''' 27 | verify_calibraion 28 | ''' 29 | 30 | 31 | def __init__(self, node): 32 | super(verify_calibraionSM, self).__init__() 33 | self.name = 'verify_calibraion' 34 | self.node = node 35 | 36 | # parameters of this behavior 37 | GetArMarkerState.initialize_ros(node) 38 | InitialPoseState.initialize_ros(node) 39 | MoveitPlanExecuteState.initialize_ros(node) 40 | ObjTransToArmState.initialize_ros(node) 41 | OperatableStateMachine.initialize_ros(node) 42 | Logger.initialize(node) 43 | self.add_parameter('eye_in_hand_mode', False) 44 | self.add_parameter('base_link', '/base_link') 45 | self.add_parameter('tip_link', '/tool0_controller') 46 | self.add_parameter('group_name', 'manipulator') 47 | self.add_parameter('reference_frame', 'base_link') 48 | 49 | # references to used behaviors 50 | 51 | # Additional initialization code can be added inside the following tags 52 | # [MANUAL_INIT] 53 | 54 | # [/MANUAL_INIT] 55 | 56 | # Behavior comments: 57 | 58 | 59 | 60 | def create(self): 61 | # x:48 y:501, x:58 y:229 62 | _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) 63 | 64 | # Additional creation code can be added inside the following tags 65 | # [MANUAL_CREATE] 66 | 67 | # [/MANUAL_CREATE] 68 | 69 | 70 | with _state_machine: 71 | # x:250 y:76 72 | OperatableStateMachine.add('get_obj_position', 73 | GetArMarkerState(eye_in_hand_mode=self.eye_in_hand_mode), 74 | transitions={'done': 'obj_to_arm_base', 'failed': 'failed'}, 75 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, 76 | remapping={'camera_h_charuco': 'camera_h_charuco'}) 77 | 78 | # x:551 y:180 79 | OperatableStateMachine.add('excute_moveit_plan', 80 | MoveitPlanExecuteState(group_name=self.group_name, reference_frame=self.reference_frame), 81 | transitions={'received': 'excute_moveit_plan', 'done': 'back_home', 'collision': 'failed'}, 82 | autonomy={'received': Autonomy.Off, 'done': Autonomy.Off, 'collision': Autonomy.Off}, 83 | remapping={'excute_position': 'excute_position', 'result_compute': 'result_compute'}) 84 | 85 | # x:564 y:83 86 | OperatableStateMachine.add('obj_to_arm_base', 87 | ObjTransToArmState(eye_in_hand_mode=self.eye_in_hand_mode, base_link=self.base_link, tip_link=self.tip_link), 88 | transitions={'done': 'excute_moveit_plan', 'failed': 'failed'}, 89 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, 90 | remapping={'camera_h_charuco': 'camera_h_charuco', 'excute_position': 'excute_position'}) 91 | 92 | # x:294 y:315 93 | OperatableStateMachine.add('back_home', 94 | InitialPoseState(group_name=self.group_name, reference_frame=self.reference_frame), 95 | transitions={'done': 'finished', 'collision': 'failed'}, 96 | autonomy={'done': Autonomy.Off, 'collision': Autonomy.Off}) 97 | 98 | 99 | return _state_machine 100 | 101 | 102 | # Private functions can be added inside the following tags 103 | # [MANUAL_FUNC] 104 | 105 | # [/MANUAL_FUNC] 106 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/find_charuco.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from flexbe_core import EventState, Logger 3 | from geometry_msgs.msg import Transform 4 | from visp_hand2eye_calibration.msg import TransformArray 5 | import time 6 | import rclpy 7 | from rclpy.clock import Clock, ClockType 8 | from tf2_ros import TransformException 9 | from tf2_ros.buffer import Buffer 10 | from tf2_ros.transform_listener import TransformListener 11 | # import tf 12 | 13 | class FindCharucoState(EventState): 14 | """ 15 | Output a fixed pose to move. 16 | 17 | <= done Charuco pose has been received. 18 | <= go_compute Ready to compute the result. 19 | 20 | """ 21 | 22 | def __init__(self, base_link, tip_link, eye_in_hand_mode): 23 | """Constructor""" 24 | super(FindCharucoState, self).__init__(outcomes=['done', 'go_compute'], input_keys=['result_compute'], 25 | output_keys=['base_h_tool', 'camera_h_charuco']) 26 | self.base_link = base_link 27 | self.tip_link = tip_link 28 | self.tf_buffer = Buffer() 29 | self.tf_listener = TransformListener(self.tf_buffer, self._node) 30 | self.base_h_tool = TransformArray() 31 | self.camera_h_charuco = TransformArray() 32 | self.base_h_tool.header.frame_id = self.base_link 33 | self.camera_h_charuco.header.frame_id = 'calib_camera' 34 | self.enter_time = Clock(clock_type=ClockType.ROS_TIME).now() 35 | self.eye_in_hand_mode = eye_in_hand_mode 36 | 37 | def execute(self, userdata): 38 | time.sleep(1.5) 39 | if (Clock(clock_type=ClockType.ROS_TIME).now() - self.enter_time).nanoseconds / 1000000000 > 2: 40 | Logger.logwarn('Can not get charuco board pose, abandon this position') 41 | return 'done' 42 | 43 | 44 | if self.eye_in_hand_mode: 45 | try: 46 | camera_trans_charuco = self.tf_buffer.lookup_transform( 47 | 'calib_camera', 48 | 'calib_charuco', 49 | rclpy.time.Time()) 50 | print(camera_trans_charuco) 51 | except TransformException as ex: 52 | Logger.logwarn('lookupTransform for charuco failed!') 53 | return 54 | else: 55 | try: 56 | camera_trans_charuco = self.tf_buffer.lookup_transform( 57 | 'calib_charuco', 58 | 'calib_camera', 59 | rclpy.time.Time()) 60 | except TransformException as ex: 61 | Logger.logwarn('lookupTransform for charuco failed!') 62 | return 63 | 64 | try: 65 | base_trans_tool = self.tf_buffer.lookup_transform( 66 | self.base_link, 67 | self.tip_link, 68 | rclpy.time.Time()) 69 | except TransformException as ex: 70 | Logger.logwarn('lookupTransform for robot failed!') 71 | return 72 | 73 | 74 | trans = Transform() 75 | trans.translation.x = camera_trans_charuco.transform.translation.x 76 | trans.translation.y = camera_trans_charuco.transform.translation.y 77 | trans.translation.z = camera_trans_charuco.transform.translation.z 78 | trans.rotation.x = camera_trans_charuco.transform.rotation.x 79 | trans.rotation.y = camera_trans_charuco.transform.rotation.y 80 | trans.rotation.z = camera_trans_charuco.transform.rotation.z 81 | trans.rotation.w = camera_trans_charuco.transform.rotation.w 82 | self.camera_h_charuco.transforms.append(trans) 83 | # print(self.camera_h_charuco.transforms) 84 | # print(self.camera_h_charuco) 85 | # print(self.camera_h_charuco.transforms[0].translation.x) 86 | # print(self.camera_h_charuco.transforms[0].translation.y) 87 | # print(self.camera_h_charuco.transforms[0].translation.z) 88 | print("============================") 89 | 90 | trans = Transform() 91 | trans.translation.x = base_trans_tool.transform.translation.x 92 | trans.translation.y = base_trans_tool.transform.translation.y 93 | trans.translation.z = base_trans_tool.transform.translation.z 94 | trans.rotation.x = base_trans_tool.transform.rotation.x 95 | trans.rotation.y = base_trans_tool.transform.rotation.y 96 | trans.rotation.z = base_trans_tool.transform.rotation.z 97 | trans.rotation.w = base_trans_tool.transform.rotation.w 98 | self.base_h_tool.transforms.append(trans) 99 | # print(self.base_h_tool.transforms) 100 | print("============================") 101 | 102 | 103 | if userdata.result_compute: 104 | userdata.base_h_tool = self.base_h_tool 105 | userdata.camera_h_charuco = self.camera_h_charuco 106 | return 'go_compute' 107 | else: 108 | return 'done' 109 | 110 | def on_enter(self, userdata): 111 | self.enter_time = Clock(clock_type=ClockType.ROS_TIME).now() 112 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/moveit_plan_excute.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import moveit_commander 5 | from moveit_msgs.msg import MoveItErrorCodes 6 | from math import pi, radians 7 | from std_msgs.msg import String 8 | import geometry_msgs.msg 9 | from tf import transformations as tf 10 | 11 | from flexbe_core import EventState, Logger 12 | 13 | from flexbe_core.proxy import ProxyActionClient 14 | 15 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 16 | # from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction, JointTrajectoryControllerState, FollowJointTrajectoryResult 17 | 18 | ''' 19 | Created on 15.06.2015 20 | 21 | @author: Philipp Schillinger 22 | ''' 23 | 24 | class MoveitPlanExecuteState(EventState): 25 | ''' 26 | Move robot by planned trajectory. 27 | 28 | -- group_name string move group name 29 | 30 | ># joint_trajectory JointTrajectory planned trajectory 31 | 32 | <= done Robot move done. 33 | <= failed Robot move failed. 34 | <= collision Robot during collision. 35 | ''' 36 | 37 | 38 | def __init__(self, group_name,reference_frame): 39 | ''' 40 | Constructor 41 | ''' 42 | super(MoveitPlanExecuteState, self).__init__(outcomes=['done', 'collision'], 43 | input_keys=['excute_position'], 44 | output_keys=['result_compute']) 45 | # group_name = "" 46 | self._group_name = group_name 47 | self._reference_frame = reference_frame 48 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 49 | self._result = MoveItErrorCodes.FAILURE 50 | self._move_group.set_pose_reference_frame(self._reference_frame) 51 | self._end_effector_link = self._move_group.get_end_effector_link() 52 | self._move_group.set_end_effector_link(self._end_effector_link) 53 | self._move_group.set_max_acceleration_scaling_factor(0.1) 54 | self._move_group.set_max_velocity_scaling_factor(0.1) 55 | self._first_joints= self._move_group.get_current_joint_values() 56 | self._first_pose = self._move_group.get_current_pose() 57 | self.points_num = 0 58 | self._execute_times = 0 59 | self.plan, self.fraction =0,0 60 | 61 | def on_start(self): 62 | # back to first pose 63 | # input() 64 | # self._result = self._move_group.go(self._first_joints, wait=True) 65 | # self._move_group.stop() 66 | # self._move_group.clear_pose_targets() 67 | # self._execute_times += 1 68 | # self.points_num = np.size(userdata.hand_eye_points) 69 | # print(np.size(userdata.hand_eye_points)) 70 | pass 71 | 72 | def execute(self, userdata): 73 | ''' 74 | Execute this state 75 | ''' 76 | 77 | new_pose = self._move_group.get_current_pose() 78 | 79 | pose_goal = geometry_msgs.msg.Pose() 80 | 81 | if np.size(userdata.excute_position["x"]) == 0: 82 | pose_goal.position.x = self._first_pose.pose.position.x 83 | pose_goal.position.y = self._first_pose.pose.position.y 84 | pose_goal.position.z = self._first_pose.pose.position.z 85 | 86 | else: 87 | pose_goal.position.x = userdata.excute_position["x"][self._execute_times] 88 | pose_goal.position.y = userdata.excute_position["y"][self._execute_times] 89 | pose_goal.position.z = userdata.excute_position["z"][self._execute_times] 90 | 91 | 92 | pose_goal.orientation.x = userdata.excute_position["qx"][self._execute_times] 93 | pose_goal.orientation.y = userdata.excute_position["qy"][self._execute_times] 94 | pose_goal.orientation.z = userdata.excute_position["qz"][self._execute_times] 95 | pose_goal.orientation.w = userdata.excute_position["qw"][self._execute_times] 96 | 97 | 98 | 99 | 100 | 101 | print(pose_goal) 102 | input() 103 | self._move_group.set_pose_target(pose_goal, self._end_effector_link) 104 | 105 | self._result = self._move_group.go(wait=True) 106 | self._move_group.stop() 107 | self._move_group.clear_pose_targets() 108 | 109 | userdata.result_compute = self._execute_times >= self.points_num 110 | 111 | 112 | 113 | if self._result == MoveItErrorCodes.SUCCESS: 114 | self._execute_times += 1 115 | return 'done' 116 | 117 | elif self._result == MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE: 118 | return 'collision' 119 | 120 | 121 | def on_enter(self, userdata): 122 | pass 123 | 124 | def on_stop(self): 125 | pass 126 | 127 | def on_pause(self): 128 | pass 129 | 130 | def on_resume(self, userdata): 131 | pass 132 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/take_picture.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from flexbe_core import EventState, Logger 3 | import cv2 4 | import pyrealsense2 as rs 5 | import numpy as np 6 | import os 7 | from ament_index_python.packages import get_package_share_directory 8 | 9 | 10 | 11 | class TakePictureState(EventState): 12 | """ 13 | Output a fixed pose to move. 14 | 15 | <= done Charuco pose has been received. 16 | <= failed Failed to get result. 17 | 18 | """ 19 | 20 | def __init__(self, pic_num, camera_type): 21 | """Constructor""" 22 | super(TakePictureState, self).__init__(outcomes=['done', 'failed']) 23 | self.excu_num = 1 24 | self.pic_num = pic_num 25 | if camera_type == 'realsense': 26 | # Configure depth and color streams 27 | self.pipeline = rs.pipeline() 28 | self.config = rs.config() 29 | # self.config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) 30 | self.config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30) 31 | 32 | # Start streaming 33 | self.cfg = self.pipeline.start(self.config) 34 | # self.pipeline.poll_for_frames 35 | # self.dev = cfg.get_device() 36 | # self.depth_sensor = dev.first_depth_sensor() 37 | # self.depth_sensor.set_option(rs.option.visual_preset, 4) 38 | 39 | self.iteration = 0 40 | self.preset = 0 41 | self.preset_name = "" 42 | self.capture = cv2.VideoCapture(1) 43 | 44 | self.save_pwd = get_package_share_directory('charuco_detector') + '/config/camera_calibration/pic/' 45 | Logger.logwarn(self.save_pwd) 46 | 47 | 48 | 49 | def on_start(self): 50 | 51 | pass 52 | def execute(self, userdata): 53 | 54 | 55 | while(True): 56 | 57 | # Wait for a coherent pair of frames: depth and color 58 | frames = self.pipeline.wait_for_frames() 59 | 60 | #depth_frame = frames.get_depth_frame() 61 | 62 | color_frame = frames.get_color_frame() 63 | #iteration = iteration + 1 64 | #if iteration > 100: 65 | #preset = preset + 1 66 | #iteration = 0 67 | #range = depth_sensor.get_option_range(rs.option.visual_preset) 68 | #preset = preset % range.max 69 | #depth_sensor.set_option(rs.option.visual_preset, preset) 70 | #preset_name = depth_sensor.get_option_value_description(rs.option.visual_preset, preset) 71 | # Convert images to numpy arrays 72 | #depth_image = np.asanyarray(depth_frame.get_data()) 73 | color_image = np.asanyarray(color_frame.get_data()) 74 | # Apply colormap on depth image (image must be converted to 8-bit per pixel first) 75 | #depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, None, 0.5, 0), cv2.COLORMAP_JET) 76 | # Stack both images horizontally 77 | #images = np.hstack((color_image, depth_colormap)) 78 | images = color_image 79 | #font = cv2.FONT_HERSHEY_SIMPLEX 80 | #cv2.putText(images, preset_name,(720,1300), font, 4,(255,255,255),2,cv2.LINE_AA) 81 | # Show images 82 | cv2.namedWindow('preview', cv2.WINDOW_AUTOSIZE) 83 | ret, RealSense = self.capture.read() 84 | cv2.imshow('preview', images) 85 | #cv2.imshow('Real', images) 86 | key = cv2.waitKey(1) 87 | if key == 13 : #enter 88 | Logger.logwarn("----------------------------------------------") 89 | # cv2.imwrite("./../../../config/pic/camera-pic-of-charucoboard-"+str(self.excu_num)+".jpg",images) 90 | cv2.imwrite(self.save_pwd+"camera-pic-of-charucoboard-"+str(self.excu_num)+".jpg",images) 91 | self.excu_num += 1 92 | #cv2.imshow('frame', frame) 93 | elif key & 0xFF == ord('q')or key == 27: # q or esc 94 | Logger.logwarn("==========================================") 95 | self.excu_num = self.pic_num 96 | return "failed" 97 | 98 | elif self.excu_num >= self.pic_num+1: 99 | self.capture.release() 100 | cv2.destroyAllWindows() 101 | # Stop streaming 102 | self.pipeline.stop() 103 | 104 | return 'done' 105 | 106 | def on_enter(self, userdata): 107 | self.enter_time = TakePictureState._node.get_clock().now() 108 | print(self.enter_time) 109 | pass 110 | 111 | 112 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/hand_eye_flexbe_behaviors/hand_eye_calibration_sm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | ########################################################### 4 | # WARNING: Generated code! # 5 | # ************************** # 6 | # Manual changes may get lost if file is generated again. # 7 | # Only code inside the [MANUAL] tags will be kept. # 8 | ########################################################### 9 | 10 | from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger 11 | from hand_eye_flexbe_states.compute_calib import ComputeCalibState 12 | from hand_eye_flexbe_states.execute_traj import TrajectoryExecuteState 13 | from hand_eye_flexbe_states.find_charuco import FindCharucoState 14 | from hand_eye_flexbe_states.get_calib_pose import GetCalibPoseState 15 | from hand_eye_flexbe_states.move_charuco_center import MoveCharucoCenterState 16 | from hand_eye_flexbe_states.pose_plan import PosePlanState 17 | # Additional imports can be added inside the following tags 18 | # [MANUAL_IMPORT] 19 | 20 | # [/MANUAL_IMPORT] 21 | 22 | 23 | ''' 24 | Created on Tue Nov 09 2021 25 | @author: Andy Chien 26 | ''' 27 | class HandEyeCalibrationSM(Behavior): 28 | ''' 29 | Used to calibrate the camera on hand or in hand 30 | ''' 31 | 32 | 33 | def __init__(self, node): 34 | super(HandEyeCalibrationSM, self).__init__() 35 | self.name = 'Hand Eye Calibration' 36 | self.node = node 37 | 38 | # parameters of this behavior 39 | ComputeCalibState.initialize_ros(node) 40 | TrajectoryExecuteState.initialize_ros(node) 41 | FindCharucoState.initialize_ros(node) 42 | GetCalibPoseState.initialize_ros(node) 43 | MoveCharucoCenterState.initialize_ros(node) 44 | PosePlanState.initialize_ros(node) 45 | OperatableStateMachine.initialize_ros(node) 46 | Logger.initialize(node) 47 | self.add_parameter('eye_in_hand', False) 48 | self.add_parameter('group_name', 'manipulator') 49 | 50 | # references to used behaviors 51 | 52 | # Additional initialization code can be added inside the following tags 53 | # [MANUAL_INIT] 54 | 55 | # [/MANUAL_INIT] 56 | 57 | # Behavior comments: 58 | 59 | 60 | 61 | def create(self): 62 | # x:421 y:521, x:130 y:365 63 | _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) 64 | 65 | # Additional creation code can be added inside the following tags 66 | # [MANUAL_CREATE] 67 | 68 | # [/MANUAL_CREATE] 69 | 70 | 71 | with _state_machine: 72 | # x:47 y:115 73 | OperatableStateMachine.add('Find Charuco To Move', 74 | FindCharucoState(), 75 | transitions={'done': 'Move Charuco Center', 'fail': 'failed'}, 76 | autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}) 77 | 78 | # x:822 y:289 79 | OperatableStateMachine.add('Execute Traj', 80 | TrajectoryExecuteState(group_name=self.group_name), 81 | transitions={'done': 'Find Charuco', 'failed': 'failed', 'collision': 'Pose Plan'}, 82 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'collision': Autonomy.Off}, 83 | remapping={'joint_trajectory': 'joint_trajectory', 'target_joints': 'target_joints', 'joint_config': 'joint_config'}) 84 | 85 | # x:634 y:114 86 | OperatableStateMachine.add('Execute Traj To Center', 87 | TrajectoryExecuteState(group_name=self.group_name), 88 | transitions={'done': 'Get Calib Pose', 'failed': 'failed', 'collision': 'Pose Plan To Center'}, 89 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'collision': Autonomy.Off}, 90 | remapping={'joint_trajectory': 'joint_trajectory', 'target_joints': 'target_joints', 'joint_config': 'joint_config'}) 91 | 92 | # x:559 y:442 93 | OperatableStateMachine.add('Find Charuco', 94 | FindCharucoState(), 95 | transitions={'done': 'Get Calib Pose', 'fail': 'failed'}, 96 | autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}) 97 | 98 | # x:322 y:314 99 | OperatableStateMachine.add('Get Calib Pose', 100 | GetCalibPoseState(), 101 | transitions={'done': 'Pose Plan', 'fail': 'Calib Compute'}, 102 | autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}) 103 | 104 | # x:210 y:115 105 | OperatableStateMachine.add('Move Charuco Center', 106 | MoveCharucoCenterState(), 107 | transitions={'done': 'Pose Plan To Center', 'fail': 'failed'}, 108 | autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}) 109 | 110 | # x:571 y:261 111 | OperatableStateMachine.add('Pose Plan', 112 | PosePlanState(group_name=self.group_name), 113 | transitions={'failed': 'Get Calib Pose', 'done': 'Execute Traj'}, 114 | autonomy={'failed': Autonomy.Off, 'done': Autonomy.Off}, 115 | remapping={'joint_trajectory': 'joint_trajectory', 'target_joints': 'target_joints'}) 116 | 117 | # x:404 y:119 118 | OperatableStateMachine.add('Pose Plan To Center', 119 | PosePlanState(group_name=self.group_name), 120 | transitions={'failed': 'failed', 'done': 'Execute Traj To Center'}, 121 | autonomy={'failed': Autonomy.Off, 'done': Autonomy.Off}, 122 | remapping={'joint_trajectory': 'joint_trajectory', 'target_joints': 'target_joints'}) 123 | 124 | # x:320 y:445 125 | OperatableStateMachine.add('Calib Compute', 126 | ComputeCalibState(), 127 | transitions={'done': 'finished', 'fail': 'failed'}, 128 | autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}) 129 | 130 | 131 | return _state_machine 132 | 133 | 134 | # Private functions can be added inside the following tags 135 | # [MANUAL_FUNC] 136 | 137 | # [/MANUAL_FUNC] 138 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/obj_trans_to_arm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import copy, os 4 | import configparser 5 | import numpy as np 6 | import math as m 7 | from visp_hand2eye_calibration.msg import TransformArray 8 | from math import pi, radians 9 | from std_msgs.msg import String 10 | from geometry_msgs.msg import Transform 11 | import tf 12 | # from tf import transformations 13 | from visp_hand2eye_calibration.msg import TransformArray 14 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 15 | from flexbe_core import EventState, Logger 16 | from flexbe_core.proxy import ProxyActionClient 17 | from flexbe_core.proxy import ProxyServiceCaller 18 | 19 | from charuco_detector.srv import eye2base, eye2baseRequest, eye2baseResponse 20 | # from charuco_detector import HandEyeTrans 21 | 22 | 23 | 24 | class obj_info(dict): 25 | 26 | def __init__(self): 27 | self['x'] = [] 28 | self['y'] = [] 29 | self['z'] = [] 30 | self['qx'] = [] 31 | self['qy'] = [] 32 | self['qz'] = [] 33 | self['qw'] = [] 34 | 35 | 36 | class ObjTransToArmState(EventState): 37 | """ 38 | Output obj pose for arm. 39 | 40 | <= done points has been created. 41 | <= failed create points fail. 42 | 43 | """ 44 | 45 | 46 | def __init__(self, eye_in_hand_mode, base_link, tip_link): 47 | ''' 48 | Constructor 49 | ''' 50 | super(ObjTransToArmState, self).__init__(outcomes=['done', 'failed'], 51 | input_keys=['camera_h_charuco'], 52 | output_keys=['excute_position']) 53 | # if eye_in_hand_mode: 54 | # self.eye_in_hand_mode = 1 55 | # else: 56 | # self.eye_in_hand_mode = -1 57 | self.eye_in_hand_mode = eye_in_hand_mode 58 | self.base_link = base_link 59 | self.tip_link = tip_link 60 | self.tf_listener = tf.TransformListener() 61 | # self.tool_h_base = TransformArray() 62 | self.First_charuco_array = [] 63 | self._origin_euler = [0, 0, 0] 64 | 65 | self.save_pwd = os.path.join(os.path.dirname(__file__), '..','..','..','charuco_detector/','config/','hand_eye_calibration/') 66 | 67 | self.trans2robot_service = '/eye_trans2base' 68 | self.trans2robot_client = ProxyServiceCaller({self.trans2robot_service: eye2base}) 69 | self.pos2robot_service = '/eye2base' 70 | self.pos2robot_client = ProxyServiceCaller({self.pos2robot_service: eye2base}) 71 | self.pix2robot_service = '/pix2base' 72 | self.pix2robot_client = ProxyServiceCaller({self.pix2robot_service: eye2base}) 73 | 74 | 75 | self.quaternion = [] 76 | self.excute_pos = [] 77 | 78 | 79 | 80 | 81 | def on_start(self): 82 | pass 83 | 84 | def execute(self, userdata): 85 | ''' 86 | Execute this state 87 | ''' 88 | 89 | userdata.excute_position = obj_info() 90 | userdata.excute_position['x'].append(self.excute_pos[0]) 91 | userdata.excute_position['y'].append(self.excute_pos[1]) 92 | userdata.excute_position['z'].append(self.excute_pos[2]+0.20265) 93 | userdata.excute_position['qx'].append(self.quaternion[0]) 94 | userdata.excute_position['qy'].append(self.quaternion[1]) 95 | userdata.excute_position['qz'].append(self.quaternion[2]) 96 | userdata.excute_position['qw'].append(self.quaternion[3]) 97 | 98 | 99 | print(userdata.excute_position) 100 | return 'done' 101 | 102 | def on_enter(self, userdata): 103 | 104 | 105 | print(userdata.camera_h_charuco.transforms[0].translation) 106 | 107 | origindegree = list(euler_from_quaternion([userdata.camera_h_charuco.transforms[0].rotation.x, 108 | userdata.camera_h_charuco.transforms[0].rotation.y, 109 | userdata.camera_h_charuco.transforms[0].rotation.z, 110 | userdata.camera_h_charuco.transforms[0].rotation.w])) 111 | origindegree[0] = origindegree[0]/3.14*180.0 112 | origindegree[1] = origindegree[1]/3.14*180.0 113 | origindegree[2] = origindegree[2]/3.14*180.0 114 | # print(origindegree) 115 | 116 | 117 | obj_pose = self.tf_listener.fromTranslationRotation((userdata.camera_h_charuco.transforms[0].translation.x, 118 | userdata.camera_h_charuco.transforms[0].translation.y, 119 | userdata.camera_h_charuco.transforms[0].translation.z) 120 | ,(userdata.camera_h_charuco.transforms[0].rotation.x, 121 | userdata.camera_h_charuco.transforms[0].rotation.y, 122 | userdata.camera_h_charuco.transforms[0].rotation.z, 123 | userdata.camera_h_charuco.transforms[0].rotation.w)) 124 | 125 | 126 | 127 | ################################################################################## 128 | thetax = np.radians(180) 129 | thetay = np.radians(180) 130 | thetaz = np.radians(-90) 131 | Rx = np.matrix([[ 1, 0 , 0 ,0], 132 | [ 0, m.cos(thetax),-m.sin(thetax),0], 133 | [ 0, m.sin(thetax), m.cos(thetax),0], 134 | [ 0, 0 , 0 ,1]]) 135 | 136 | Ry = np.matrix([[ m.cos(thetay), 0, m.sin(thetay) ,0], 137 | [ 0 , 1, 0 ,0], 138 | [-m.sin(thetay), 0, m.cos(thetay) ,0], 139 | [ 0 , 0,0 ,1]]) 140 | 141 | Rz = np.matrix([[ m.cos(thetaz), -m.sin(thetaz), 0 ,0], 142 | [ m.sin(thetaz), m.cos(thetaz) , 0 ,0], 143 | [ 0 , 0 , 1 ,0], 144 | [ 0 , 0 , 0 ,1]]) 145 | 146 | ############################################################################### 147 | 148 | if self.eye_in_hand_mode: 149 | 150 | obj_pose = obj_pose * Rx 151 | # 152 | else: 153 | obj_pose = obj_pose * Ry 154 | 155 | obj_pose = list(obj_pose.flat) 156 | print(obj_pose) 157 | 158 | 159 | 160 | 161 | req = eye2baseRequest() 162 | req.ini_pose = obj_pose 163 | 164 | try: 165 | res = self.trans2robot_client.call(self.trans2robot_service, req) 166 | except rospy.ServiceException as e: 167 | Logger.loginfo("Service call failed: %s" % e) 168 | return 'failed' 169 | 170 | self.excute_pos = res.pos 171 | 172 | self.quaternion = [res.quat[0], res.quat[1], res.quat[2], res.quat[3]] 173 | 174 | 175 | origindegree = list(euler_from_quaternion(self.quaternion)) 176 | origindegree[0] = origindegree[0]/3.14*180.0 177 | origindegree[1] = origindegree[1]/3.14*180.0 178 | origindegree[2] = origindegree[2]/3.14*180.0 179 | 180 | print(origindegree) 181 | 182 | 183 | 184 | pass 185 | 186 | def on_stop(self): 187 | pass 188 | 189 | def on_pause(self): 190 | pass 191 | 192 | def on_resume(self, userdata): 193 | # self.on_enter(userdata) 194 | pass 195 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Universal Automatical Hand-Eye-Calibration 2 | This Hand-eye_calibration method include [FlexBE](http://philserver.bplaced.net/fbe/download.php), and [MoveIt!2](https://moveit.ros.org/), which make this whole framework easier for beginner to do hand-eye-calibration, also implement on different platform more faster. 3 | 4 | ## Acknowledgment 5 | This charuco marker detection is forked from the [charuco_detector](https://github.com/carlosmccosta/charuco_detector). 6 | And edit it to ROS2 version. 7 | 8 | ## Requirements 9 | This package requires a system setup with ROS. It is recommended to use **Ubuntu 22.04 with ROS 10 | humble**. 11 | 12 | camera SDK and ROS2 package is needed. Here we use [Linux Distribution](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) and [Realsense-ros](https://github.com/IntelRealSense/realsense-ros/tree/ros2-development) 13 | 14 | 15 | To make sure that program isn't affected by python version, it is highly recommended to use a docker, 16 | we have build a environment that all Requirement for this package is build, 17 | See the [ubuntu-docker](https://github.com/errrr0501/ubuntu_docker) on information how to set this up. 18 | 19 | ## Building 20 | 21 | ```bash 22 | 23 | # Install MoveIt 24 | sudo apt install ros-$ROS_DISTRO--moveit 25 | 26 | # Clone this hand-eye-cliabtion package, flexbe_engine, flexbe_app, charuco_detector to your workspace 27 | git clone --recursive https://github.com/tku-iarc/hand-eye-calibration_ROS2.git 28 | 29 | # build the workspace 30 | colcon build --symlink-install 31 | 32 | # activate the workspace (ie: source it) 33 | source install/setup.bash 34 | ``` 35 | ## Alternative: Robot arm package 36 | Note: MoveIt has been used on over 126 robots by the community, in this automatical hand-eye-calibration method, 37 | we chose it to do robot control and motion planning, therefore, user needs to use their own robot and it's MoveIt package. 38 | For example, we use [Universal Robot 5 e-Series](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/humble) as sample. 39 | 40 | ## Setting up a robot for hand-eye-calibration 41 | 42 | ### Prepare the camera also do camera calibration first 43 | Note:In our method we use Realsense camera model and do camera calibration with charuco marker board. 44 | No matter what kind of calibration method just make sure you have Distortion parameter and Intrinsic parameter, 45 | like we put in [directory](https://github.com/errrr0501/charuco_detector_ROS2/tree/main/charuco_detector/config/camera_calibration). 46 | ### Camera calibration 47 | we call Realsense with python, calibrate with charuco marker board, and everything is under FlexBE. 48 | If you want to use, call FlexBE: 49 | 50 | ```bash 51 | ros2 launch flexbe_app flexbe_full.launch 52 | ``` 53 | 54 | Then, press `Load Behavior` on the top, and select `camera_calibration` in left window. 55 | 56 | After this, you can press `Runtime Control` on the top, execution window will show: 57 | 58 |
camera_calibration_execution
59 | 60 | Before press `Start Execution`, parameter `pic_num` decide how many calibration picture you will take, 61 | parameter `square_size`, `marker_size` , `col_count` , `row_count` means the spec of charuco marker board we use. 62 | parameter `save_file_name` can let you change your result file name. 63 | 64 | ### Prepare the robot 65 | Activate robot and open with MoveIt. 66 | 67 | As example using Universal Robot 5 e-Series: 68 | 69 | - To start hardware, use: 70 | ```bash 71 | ros2 launch ur_robot_driver ur_control.launch.py ur_type:= robot_ip:= launch_rviz:=false 72 | ``` 73 | - To do test with moveIt, use: 74 | ```bash 75 | ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e launch_rviz:=true 76 | 77 | ``` 78 | 79 | #### Quick start 80 | ### For using manual hand eye calibration 81 | Note:Here we use Realsense D435i+UR5e, 82 | we use whole method with FlexBE and ROS2, open camera with ROS2 first: 83 | - Open camera 84 | ```bash 85 | cd ~/ 86 | source install/setup.bash 87 | ros2 launch realsense2_camera rs_launch.py rgb_camera.profile:=1920x1080x30 88 | ``` 89 | WARN:Here use 1080p as camera resolution, because we use 1080p as camera calibration resolution, 90 | if you want to change resolution, need to do camera calibration again with the resolution you change. 91 | 92 | - Then launch flexbe_app flexbe_full.launch. 93 | ```bash 94 | ros2 launch flexbe_app flexbe_full.launch 95 | ``` 96 | - Open charuco detector 97 | ```bash 98 | ros2 launch charuco_detector hand_eye_calibration.launch.py 99 | ``` 100 | - Open visp_hand2eye_calibration server 101 | ```bash 102 | ros2 run visp_hand2eye_calibration visp_hand2eye_calibration_calibrator 103 | ``` 104 | Note: In ros.yaml file we save in `charuco_detector/config/` 105 | parameter `squaresSidesSizeM` is for for charuco board's squares side size, default is `0.0200` 106 | parameter `markersSidesSizeM` is for for charuco board's markers side size, default is `0.0150` 107 | parameter `numberOfBitsForMarkersSides` is for for charuco board's markers side size, default is `4` 108 | parameter `numberOfMarkers` is for for charuco board's markers side size, default is `70` 109 | parameter `numberOfSquaresInX` is for for charuco board's markers side size, default is `10` 110 | parameter `numberOfSquaresInY` is for for charuco board's markers side size, default is `14` 111 | parameter `dictionaryId` is for for charuco board's markers side size, default is `3` 112 | parameter `image_topic` is for recieve camera image on ROS, default is `/camera/color/image_raw` 113 | parameter `camera_info` is for recieve camera info on ROS, default is `/camera/color/camera_info` 114 | 115 | Must colcon build --symlink-install after edit config file. 116 | 117 | Then, press `Load Behavior` on the top, and select `Manual Hand Eye Calibration` in left window. 118 | 119 | After this, you can press `Runtime Control` on the top, execution window will show: 120 | 121 |
maunal_hand_eye_calib_execute
122 | 123 | Before press `Start Execution`, parameter `eye_in_hand` decide which calibration mode you want to use, 124 | `calib_pose_num` means how many sample points you want, 125 | `base_link` , `tip_link` means the base coordinate and end-effector coordinate's name, 126 | `calibration_file_name` means the name of save file. `move_distance` means the distance of each sample points, 127 | parameter `customize_file` , decide whether you want to change save file name or not. 128 | 129 | ## Result file 130 | 131 | Result will save in `install/charuco_detector/share/charuco_detector/config/hand_eye_calibration/` . 132 | -------------------------------------------------------------------------------- /hand_eye_flexbe_behaviors/hand_eye_flexbe_behaviors/automatic_hand_eye_calibration_sm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | ########################################################### 4 | # WARNING: Generated code! # 5 | # ************************** # 6 | # Manual changes may get lost if file is generated again. # 7 | # Only code inside the [MANUAL] tags will be kept. # 8 | ########################################################### 9 | 10 | from flexbe_core import Behavior, Autonomy, OperatableStateMachine, ConcurrencyContainer, PriorityContainer, Logger 11 | from hand_eye_flexbe_states.compute_calib import ComputeCalibState 12 | from hand_eye_flexbe_states.find_charuco import FindCharucoState 13 | from hand_eye_flexbe_states.generate_hand_eye_point import GenerateHandEyePointState 14 | from hand_eye_flexbe_states.initial_pose import InitialPoseState 15 | from hand_eye_flexbe_states.move_robot_manually import MoveRobotManuallyState 16 | from hand_eye_flexbe_states.moveit_hand_eye_excute import MoveitHandEyeExecuteState 17 | # Additional imports can be added inside the following tags 18 | # [MANUAL_IMPORT] 19 | 20 | # [/MANUAL_IMPORT] 21 | 22 | 23 | ''' 24 | Created on Sun Nov 14 2021 25 | @author: Luis Tsai 26 | ''' 27 | class AutomaticHandEyeCalibrationSM(Behavior): 28 | ''' 29 | Execute hand eye calibration by manual 30 | ''' 31 | 32 | 33 | def __init__(self, node): 34 | super(AutomaticHandEyeCalibrationSM, self).__init__() 35 | self.name = 'Automatic Hand Eye Calibration' 36 | self.node = node 37 | 38 | # parameters of this behavior 39 | ComputeCalibState.initialize_ros(node) 40 | FindCharucoState.initialize_ros(node) 41 | GenerateHandEyePointState.initialize_ros(node) 42 | InitialPoseState.initialize_ros(node) 43 | MoveRobotManuallyState.initialize_ros(node) 44 | MoveitHandEyeExecuteState.initialize_ros(node) 45 | OperatableStateMachine.initialize_ros(node) 46 | Logger.initialize(node) 47 | self.add_parameter('eye_in_hand_mode', False) 48 | self.add_parameter('customize_file', False) 49 | self.add_parameter('calib_pose_num', 0) 50 | self.add_parameter('base_link', '/base_link') 51 | self.add_parameter('tip_link', '/tool0_controller') 52 | self.add_parameter('calibration_file_name', 'hand_eye_calibration.ini') 53 | self.add_parameter('move_distance', 0.10) 54 | self.add_parameter('reference_frame', 'base_link') 55 | self.add_parameter('group_name', 'manipulator') 56 | self.add_parameter('axis', 'x-y-z') 57 | self.add_parameter('points_num', 24) 58 | 59 | # references to used behaviors 60 | 61 | # Additional initialization code can be added inside the following tags 62 | # [MANUAL_INIT] 63 | 64 | # [/MANUAL_INIT] 65 | 66 | # Behavior comments: 67 | 68 | 69 | 70 | def create(self): 71 | # x:275 y:503, x:182 y:417 72 | _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) 73 | 74 | # Additional creation code can be added inside the following tags 75 | # [MANUAL_CREATE] 76 | 77 | # [/MANUAL_CREATE] 78 | 79 | 80 | with _state_machine: 81 | # x:133 y:27 82 | OperatableStateMachine.add('Move_Robot_Manually', 83 | MoveRobotManuallyState(wait_time=2, pose_num=self.calib_pose_num), 84 | transitions={'done': 'Find_First_Charuco'}, 85 | autonomy={'done': Autonomy.Off}, 86 | remapping={'result_compute': 'result_compute'}) 87 | 88 | # x:564 y:364 89 | OperatableStateMachine.add('Calibration_Computation', 90 | ComputeCalibState(eye_in_hand_mode=self.eye_in_hand_mode, calibration_file_name=self.calibration_file_name, customize_file=self.customize_file), 91 | transitions={'finish': 'Back_to_Initial_Pose'}, 92 | autonomy={'finish': Autonomy.Off}, 93 | remapping={'base_h_tool': 'base_h_tool', 'camera_h_charuco': 'camera_h_charuco'}) 94 | 95 | # x:734 y:248 96 | OperatableStateMachine.add('Find_Charuco', 97 | FindCharucoState(base_link=self.base_link, tip_link=self.tip_link, eye_in_hand_mode=self.eye_in_hand_mode), 98 | transitions={'done': 'Moveit_Execute_Points', 'go_compute': 'Calibration_Computation'}, 99 | autonomy={'done': Autonomy.Off, 'go_compute': Autonomy.Off}, 100 | remapping={'result_compute': 'result_compute', 'base_h_tool': 'base_h_tool', 'camera_h_charuco': 'camera_h_charuco'}) 101 | 102 | # x:144 y:121 103 | OperatableStateMachine.add('Find_First_Charuco', 104 | FindCharucoState(base_link=self.base_link, tip_link=self.tip_link, eye_in_hand_mode=self.eye_in_hand_mode), 105 | transitions={'done': 'Find_First_Charuco', 'go_compute': 'Generate_Points'}, 106 | autonomy={'done': Autonomy.Off, 'go_compute': Autonomy.Off}, 107 | remapping={'result_compute': 'result_compute', 'base_h_tool': 'base_h_tool', 'camera_h_charuco': 'camera_h_charuco'}) 108 | 109 | # x:83 y:243 110 | OperatableStateMachine.add('Generate_Points', 111 | GenerateHandEyePointState(eye_in_hand_mode=self.eye_in_hand_mode, base_link=self.base_link, tip_link=self.tip_link, move_distance=self.move_distance, group_name=self.group_name, reference_frame=self.reference_frame, points_num=self.points_num, axis=self.axis), 112 | transitions={'done': 'Moveit_Execute_Points', 'failed': 'failed', 'correct_pose': 'Moveit_Execute_Points', 'recheck': 'recheck_Charuco'}, 113 | autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'correct_pose': Autonomy.Off, 'recheck': Autonomy.Off}, 114 | remapping={'camera_h_charuco': 'camera_h_charuco', 'hand_eye_points': 'hand_eye_points', 'motion_state': 'motion_state', 'result_compute': 'result_compute'}) 115 | 116 | # x:468 y:112 117 | OperatableStateMachine.add('Moveit_Execute_Points', 118 | MoveitHandEyeExecuteState(group_name=self.group_name, reference_frame=self.reference_frame, points_num=self.points_num), 119 | transitions={'received': 'Find_Charuco', 'done': 'Find_Charuco', 'finish_correction': 'Generate_Points', 'collision': 'failed'}, 120 | autonomy={'received': Autonomy.Off, 'done': Autonomy.Off, 'finish_correction': Autonomy.Off, 'collision': Autonomy.Off}, 121 | remapping={'hand_eye_points': 'hand_eye_points', 'motion_state': 'motion_state', 'result_compute': 'result_compute'}) 122 | 123 | # x:29 y:458 124 | OperatableStateMachine.add('recheck_Charuco', 125 | FindCharucoState(base_link=self.base_link, tip_link=self.tip_link, eye_in_hand_mode=self.eye_in_hand_mode), 126 | transitions={'done': 'recheck_Charuco', 'go_compute': 'Generate_Points'}, 127 | autonomy={'done': Autonomy.Off, 'go_compute': Autonomy.Off}, 128 | remapping={'result_compute': 'result_compute', 'base_h_tool': 'base_h_tool', 'camera_h_charuco': 'camera_h_charuco'}) 129 | 130 | # x:356 y:325 131 | OperatableStateMachine.add('Back_to_Initial_Pose', 132 | InitialPoseState(group_name=self.group_name, reference_frame=self.reference_frame), 133 | transitions={'done': 'finished', 'collision': 'failed'}, 134 | autonomy={'done': Autonomy.Off, 'collision': Autonomy.Off}) 135 | 136 | 137 | return _state_machine 138 | 139 | 140 | # Private functions can be added inside the following tags 141 | # [MANUAL_FUNC] 142 | 143 | # [/MANUAL_FUNC] 144 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/charuco_camera_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from flexbe_core import EventState, Logger 3 | import time 4 | import numpy 5 | import cv2 6 | from cv2 import aruco 7 | import pickle 8 | import glob,os 9 | import configparser 10 | from sensor_msgs.msg import CameraInfo 11 | from ament_index_python.packages import get_package_share_directory 12 | 13 | class CharucoCameraCalibrationState(EventState): 14 | """ 15 | Output a fixed pose to move. 16 | 17 | <= done Charuco pose has been received. 18 | <= go_compute Ready to compute the result. 19 | 20 | """ 21 | 22 | def __init__(self, square_size, marker_size, col_count, row_count, save_file_name): 23 | """Constructor""" 24 | super(CharucoCameraCalibrationState, self).__init__(outcomes=['done', 'failed']) 25 | self.camera_calibration_file = save_file_name 26 | 27 | self.CHARUCOBOARD_COLCOUNT = col_count 28 | self.CHARUCOBOARD_ROWCOUNT = row_count 29 | self.ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_4X4_100) 30 | self.squareLength = square_size 31 | self.markerLength = marker_size 32 | 33 | # Create constants to be passed into OpenCV and Aruco methods 34 | self.CHARUCO_BOARD = aruco.CharucoBoard_create( 35 | squaresX=self.CHARUCOBOARD_COLCOUNT, 36 | squaresY=self.CHARUCOBOARD_ROWCOUNT, 37 | squareLength=self.squareLength, 38 | markerLength=self.markerLength, 39 | dictionary=self.ARUCO_DICT) 40 | 41 | # Create the arrays and variables we'll use to store info like corners and IDs from images processed 42 | self.corners_all = [] # Corners discovered in all images processed 43 | self.ids_all = [] # Aruco ids corresponding to corners discovered 44 | self.image_size = None # Determined at runtime 45 | # self.save_pwd = os.path.join(os.path.dirname(__file__), '..','..','..',',','charuco_detector_ROS2/','charuco_detector/','config/','camera_calibration/') 46 | self.save_pwd = get_package_share_directory('charuco_detector') + '/config/camera_calibration/' 47 | 48 | self.images = glob.glob(self.save_pwd + 'pic/camera-pic-of-charucoboard-*.jpg') 49 | 50 | 51 | def on_start(self): 52 | pass 53 | def execute(self, userdata): 54 | for iname in self.images: 55 | # Open the image 56 | img = cv2.imread(iname) 57 | # Grayscale the image 58 | gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 59 | 60 | # Find aruco markers in the query image 61 | corners, ids, _ = aruco.detectMarkers( 62 | image=gray, 63 | dictionary=self.ARUCO_DICT) 64 | 65 | # Outline the aruco markers found in our query image 66 | img = aruco.drawDetectedMarkers( 67 | image=img, 68 | corners=corners) 69 | 70 | # Get charuco corners and ids from detected aruco markers 71 | response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( 72 | markerCorners=corners, 73 | markerIds=ids, 74 | image=gray, 75 | board=self.CHARUCO_BOARD) 76 | 77 | # If a Charuco board was found, let's collect image/corner points 78 | # Requiring at least 20 squares 79 | if response > 20: 80 | # Add these corners and ids to our calibration arrays 81 | self.corners_all.append(charuco_corners) 82 | self.ids_all.append(charuco_ids) 83 | 84 | # Draw the Charuco board we've detected to show our calibrator the board was properly detected 85 | img = aruco.drawDetectedCornersCharuco( 86 | image=img, 87 | charucoCorners=charuco_corners, 88 | charucoIds=charuco_ids) 89 | 90 | # If our image size is unknown, set it now 91 | if not self.image_size: 92 | self.image_size = gray.shape[::-1] 93 | 94 | # Reproportion the image, maxing width or height at 1000 95 | proportion = max(img.shape) / 1920.0 96 | img = cv2.resize(img, (int(img.shape[1]/proportion), int(img.shape[0]/proportion))) 97 | # Pause to display each image, waiting for key press 98 | cv2.imshow('Charuco board', img) 99 | cv2.waitKey(0) 100 | else: 101 | Logger.logwarn("Not able to detect a charuco board in image: {}".format(iname)) 102 | 103 | # Destroy any open CV windows 104 | cv2.destroyAllWindows() 105 | Logger.logwarn("==========================================================================") 106 | # Make sure at least one image was found 107 | if len(self.images) < 1: 108 | # Calibration failed because there were no images, warn the user 109 | print("Calibration was unsuccessful. No images of charucoboards were found. Add images of charucoboards and use or alter the naming conventions used in this file.") 110 | # Exit for failure 111 | return "failed" 112 | 113 | # Make sure we were able to calibrate on at least one charucoboard by checking 114 | # if we ever determined the image size 115 | if not self.image_size: 116 | # Calibration failed because we didn't see any charucoboards of the PatternSize used 117 | Logger.logwarn("Calibration was unsuccessful. We couldn't detect charucoboards in any of the images supplied. Try changing the patternSize passed into Charucoboard_create(), or try different pictures of charucoboards.") 118 | # Exit for failure 119 | return "failed" 120 | 121 | # Now that we've seen all of our images, perform the camera calibration 122 | # based on the set of points we've discovered 123 | # print("==========================================================================") 124 | calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( 125 | charucoCorners=self.corners_all, 126 | charucoIds=self.ids_all, 127 | board=self.CHARUCO_BOARD, 128 | imageSize=self.image_size, 129 | cameraMatrix=None, 130 | distCoeffs=None) 131 | 132 | # Print matrix and distortion coefficient to the console 133 | # print("==========================================================================") 134 | 135 | print("-----------------------------------------------------") 136 | print(cameraMatrix) 137 | print(distCoeffs) 138 | print("-----------------------------------------------------") 139 | config = configparser.ConfigParser() 140 | config.optionxform = str 141 | #reference: http://docs.python.org/library/configparser.html 142 | # rospack = rospkg.RosPack() 143 | # # curr_path = rospack.get_path('charuco_detector') 144 | # # config.read(curr_path + '/config/ '+ self.camera_calibration_file) 145 | 146 | config.add_section("Distortion") 147 | config.set("Distortion", "k1", str(distCoeffs[0][0])) 148 | config.set("Distortion", "k2", str(distCoeffs[0][1])) 149 | config.set("Distortion", "t1", str(distCoeffs[0][2])) 150 | config.set("Distortion", "t2", str(distCoeffs[0][3])) 151 | config.set("Distortion", "k3", str(distCoeffs[0][4])) 152 | config.add_section("Intrinsic") 153 | config.set("Intrinsic", "0_0", str(cameraMatrix[0][0])) 154 | config.set("Intrinsic", "0_1", str(cameraMatrix[0][1])) 155 | config.set("Intrinsic", "0_2", str(cameraMatrix[0][2])) 156 | config.set("Intrinsic", "1_0", str(cameraMatrix[1][0])) 157 | config.set("Intrinsic", "1_1", str(cameraMatrix[1][1])) 158 | config.set("Intrinsic", "1_2", str(cameraMatrix[1][2])) 159 | config.set("Intrinsic", "2_0", str(cameraMatrix[2][0])) 160 | config.set("Intrinsic", "2_1", str(cameraMatrix[2][1])) 161 | config.set("Intrinsic", "2_2", str(cameraMatrix[2][2])) 162 | 163 | with open(self.save_pwd+ self.camera_calibration_file, 'w') as file: 164 | config.write(file) 165 | # Save values to be used where matrix+dist is required, for instance for posture estimation 166 | # I save files in a pickle file, but you can use yaml or whatever works for you 167 | # f = open(self.save_pwd+'/calibration.pckl', 'wb') 168 | # pickle.dump((cameraMatrix, distCoeffs, rvecs, tvecs), f) 169 | # f.close() 170 | # f = open(self.save_pwd+'/calibration.yaml', 'wb') 171 | # pickle.dump((cameraMatrix, distCoeffs, rvecs, tvecs), f) 172 | # f.close() 173 | 174 | 175 | # Print to console our success 176 | print('Calibration successful. Calibration file used: {}'.format('calibration.yaml')) 177 | 178 | 179 | return 'done' 180 | 181 | def on_enter(self, userdata): 182 | self.enter_time = CharucoCameraCalibrationState._node.get_clock().now() 183 | 184 | pass 185 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/compute_calib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import configparser,os 4 | import numpy 5 | import math 6 | # import tf 7 | from geometry_msgs.msg import Transform 8 | from visp_hand2eye_calibration.msg import TransformArray 9 | from flexbe_core import EventState 10 | from flexbe_core.proxy import ProxyServiceCaller 11 | from ament_index_python.packages import get_package_share_directory 12 | from visp_hand2eye_calibration.srv import ComputeEffectorCameraQuick 13 | 14 | 15 | class ComputeCalibState(EventState): 16 | """ 17 | Output a fixed pose to move. 18 | 19 | <= done Pose has been published. 20 | <= fail Task fail and finished 21 | 22 | """ 23 | 24 | def __init__(self, eye_in_hand_mode, calibration_file_name, customize_file): 25 | """Constructor""" 26 | super(ComputeCalibState, self).__init__(outcomes=['finish'], input_keys=['base_h_tool', 'camera_h_charuco']) 27 | self.eye_in_hand_mode = eye_in_hand_mode 28 | self.camera_object_list = TransformArray() 29 | self.world_effector_list = TransformArray() 30 | ProxyServiceCaller._initialize(ComputeCalibState._node) 31 | self.calib_compute_client = ProxyServiceCaller({'/compute_effector_camera_quick':ComputeEffectorCameraQuick}) 32 | self.save_pwd = get_package_share_directory('charuco_detector') + '/config/hand_eye_calibration/' 33 | self.config = configparser.ConfigParser() 34 | self.config.optionxform = str #reference: http://docs.python.org/library/configparser.html 35 | 36 | if customize_file: 37 | self.calibration_file_name = str(calibration_file_name) 38 | with open(self.save_pwd+ self.calibration_file_name, 'w') as file: 39 | self.config.write(file) 40 | else: 41 | if eye_in_hand_mode: 42 | self.calibration_file_name = "eye_in_hand_calibration.ini" 43 | else: 44 | self.calibration_file_name = "eye_to_hand_calibration.ini" 45 | 46 | 47 | 48 | def execute(self, userdata): 49 | req = ComputeEffectorCameraQuick.Request() 50 | req.camera_object = self.camera_object_list 51 | req.world_effector = self.world_effector_list 52 | print ("========================================================================================================") 53 | res = self.calib_compute_client.call('/compute_effector_camera_quick', req) 54 | 55 | print('x = ' + str(res.effector_camera.translation.x)) 56 | print('y = ' + str(res.effector_camera.translation.y)) 57 | print('z = ' + str(res.effector_camera.translation.z)) 58 | print('qx = ' + str(res.effector_camera.rotation.x)) 59 | print('qy = ' + str(res.effector_camera.rotation.y)) 60 | print('qz = ' + str(res.effector_camera.rotation.z)) 61 | print('qw = ' + str(res.effector_camera.rotation.w)) 62 | 63 | # config = configparser.ConfigParser() 64 | # config.optionxform = str #reference: http://docs.python.org/library/configparser.html 65 | self.config.read(self.save_pwd + self.calibration_file_name) 66 | # config.read(curr_path + '/config/hand_eye_calibration/'+ self.calibration_file_name) 67 | 68 | if self.config.get("hand_eye_calibration" ,"x") != None: 69 | pass 70 | else: 71 | self.config.add_section("hand_eye_calibration") 72 | self.config.set("hand_eye_calibration", "x", str(res.effector_camera.translation.x)) 73 | self.config.set("hand_eye_calibration", "y", str(res.effector_camera.translation.y)) 74 | self.config.set("hand_eye_calibration", "z", str(res.effector_camera.translation.z)) 75 | self.config.set("hand_eye_calibration", "qx", str(res.effector_camera.rotation.x)) 76 | self.config.set("hand_eye_calibration", "qy", str(res.effector_camera.rotation.y)) 77 | self.config.set("hand_eye_calibration", "qz", str(res.effector_camera.rotation.z)) 78 | self.config.set("hand_eye_calibration", "qw", str(res.effector_camera.rotation.w)) 79 | 80 | with open(self.save_pwd+ self.calibration_file_name, 'w') as file: 81 | self.config.write(file) 82 | return 'finish' 83 | 84 | def on_enter(self, userdata): 85 | if self.eye_in_hand_mode: 86 | print("------------------------------------------------------------------") 87 | self.world_effector_list = userdata.base_h_tool 88 | self.camera_object_list = userdata.camera_h_charuco 89 | # print(self.camera_object_list) 90 | # print(self.world_effector_list) 91 | else: 92 | self.world_effector_list.header = userdata.base_h_tool.header 93 | print ("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++") 94 | print (userdata.base_h_tool) 95 | 96 | self.camera_object_list.header = userdata.camera_h_charuco.header 97 | 98 | for transform in userdata.base_h_tool.transforms: 99 | trans = self.quaternion_matrix([transform.rotation.x, transform.rotation.y, 100 | transform.rotation.z, transform.rotation.w]) 101 | trans[0:3, 3] = [transform.translation.x, transform.translation.y, transform.translation.z] 102 | trans = self.inverse_matrix(trans) 103 | trans_B = Transform() 104 | trans_B.translation.x, trans_B.translation.y, trans_B.translation.z = trans[:3, 3] 105 | trans_B.rotation.x, trans_B.rotation.y, trans_B.rotation.z, \ 106 | trans_B.rotation.w = self.quaternion_from_matrix(trans) 107 | self.world_effector_list.transforms.append(trans_B) 108 | 109 | for transform in userdata.camera_h_charuco.transforms: 110 | trans = self.quaternion_matrix([transform.rotation.x, transform.rotation.y, 111 | transform.rotation.z, transform.rotation.w]) 112 | trans[0:3, 3] = [transform.translation.x, transform.translation.y, transform.translation.z] 113 | trans = self.inverse_matrix(trans) 114 | trans_C = Transform() 115 | trans_C.translation.x, trans_C.translation.y, trans_C.translation.z = trans[:3, 3] 116 | trans_C.rotation.x, trans_C.rotation.y, trans_C.rotation.z, \ 117 | trans_C.rotation.w = self.quaternion_from_matrix(trans) 118 | self.camera_object_list.transforms.append(trans_C) 119 | 120 | def quaternion_matrix(self, quaternion): 121 | """Return homogeneous rotation matrix from quaternion. 122 | 123 | >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) 124 | >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) 125 | True 126 | 127 | """ 128 | q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) 129 | nq = numpy.dot(q, q) 130 | if nq < numpy.finfo(float).eps * 4.0: 131 | return numpy.identity(4) 132 | q *= math.sqrt(2.0 / nq) 133 | q = numpy.outer(q, q) 134 | return numpy.array(( 135 | (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), 136 | (q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), 137 | (q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), 138 | (0.0, 0.0, 0.0, 1.0) 139 | ), dtype=numpy.float64) 140 | 141 | def quaternion_from_matrix(self, matrix): 142 | """Return quaternion from rotation matrix. 143 | 144 | >>> R = rotation_matrix(0.123, (1, 2, 3)) 145 | >>> q = quaternion_from_matrix(R) 146 | >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) 147 | True 148 | 149 | """ 150 | q = numpy.empty((4, ), dtype=numpy.float64) 151 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] 152 | t = numpy.trace(M) 153 | if t > M[3, 3]: 154 | q[3] = t 155 | q[2] = M[1, 0] - M[0, 1] 156 | q[1] = M[0, 2] - M[2, 0] 157 | q[0] = M[2, 1] - M[1, 2] 158 | else: 159 | i, j, k = 0, 1, 2 160 | if M[1, 1] > M[0, 0]: 161 | i, j, k = 1, 2, 0 162 | if M[2, 2] > M[i, i]: 163 | i, j, k = 2, 0, 1 164 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 165 | q[i] = t 166 | q[j] = M[i, j] + M[j, i] 167 | q[k] = M[k, i] + M[i, k] 168 | q[3] = M[k, j] - M[j, k] 169 | q *= 0.5 / math.sqrt(t * M[3, 3]) 170 | return q 171 | 172 | def inverse_matrix(self, matrix): 173 | """Return inverse of square transformation matrix. 174 | 175 | >>> M0 = random_rotation_matrix() 176 | >>> M1 = inverse_matrix(M0.T) 177 | >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) 178 | True 179 | >>> for size in range(1, 7): 180 | ... M0 = numpy.random.rand(size, size) 181 | ... M1 = inverse_matrix(M0) 182 | ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size 183 | 184 | """ 185 | return numpy.linalg.inv(matrix) 186 | 187 | -------------------------------------------------------------------------------- /hand_eye_flexbe_states/hand_eye_flexbe_states/generate_hand_eye_point.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import copy 4 | import moveit_commander 5 | from moveit_msgs.msg import MoveItErrorCodes 6 | from visp_hand2eye_calibration.msg import TransformArray 7 | from math import pi, radians 8 | from std_msgs.msg import String 9 | from geometry_msgs.msg import Transform 10 | from moveit_commander.conversions import pose_to_list 11 | import tf 12 | # from tf import transformations 13 | from visp_hand2eye_calibration.msg import TransformArray 14 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 15 | from flexbe_core import EventState, Logger 16 | import numpy as np 17 | from flexbe_core.proxy import ProxyActionClient 18 | 19 | # from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, Constraints, JointConstraint, MoveItErrorCodes 20 | # from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction, JointTrajectoryControllerState, FollowJointTrajectoryResult 21 | 22 | # class correct_point(dict): 23 | 24 | # def __init__(self): 25 | # self['x'] = [] 26 | # self['y'] = [] 27 | # self['z'] = [] 28 | # self['qw'] = [] 29 | # self['qx'] = [] 30 | # self['qy'] = [] 31 | # self['qz'] = [] 32 | 33 | class GenerateHandEyePointState(EventState): 34 | """ 35 | Output a fixed pose to move. 36 | 37 | <= done points has been created. 38 | <= fail create points fail. 39 | 40 | """ 41 | 42 | 43 | def __init__(self, eye_in_hand_mode, base_link, tip_link, move_distance, group_name, reference_frame, points_num, axis): 44 | ''' 45 | Constructor 46 | ''' 47 | super(GenerateHandEyePointState, self).__init__(outcomes=['done', 'failed','correct_pose','recheck'], 48 | input_keys=['camera_h_charuco'], 49 | output_keys=['hand_eye_points','motion_state','result_compute']) 50 | self.move_distance = float(move_distance) 51 | self.eye_in_hand_mode = eye_in_hand_mode 52 | self.base_link = base_link 53 | self.tip_link = tip_link 54 | self.tf_listener = tf.TransformListener() 55 | # self.tool_h_base = TransformArray() 56 | self.waypoints = [] 57 | self.plan_path = [] 58 | self.execute_num = 0 59 | self.points_num = points_num 60 | self.loop_size = int(self.points_num/4) 61 | self.loop_num = 0 62 | self.First_charuco_array = [] 63 | self._group_name = group_name 64 | self._reference_frame = reference_frame 65 | self._move_group = moveit_commander.MoveGroupCommander(self._group_name) 66 | self._result = MoveItErrorCodes.FAILURE 67 | self._move_group.set_pose_reference_frame(self._reference_frame) 68 | self._end_effector_link = self._move_group.get_end_effector_link() 69 | self._first_pose = self._move_group.get_current_pose() 70 | self._first_joints= self._move_group.get_current_joint_values() 71 | self.wpose = self._move_group.get_current_pose().pose 72 | self._origin_euler = [0, 0, 0] 73 | self._base_end_axis = axis 74 | self.roll = 0 75 | self.pitch = 0 76 | self.yaw = 0 77 | self.correct_degree = [] 78 | self.cen_delta_x = 0.000 79 | self.cen_delta_y = 0.000 80 | self.cen_delta_z = 0.000 81 | self.rotation_correct = False 82 | self.centralized = False 83 | self.recheck = False 84 | self.degree_direction = 1 85 | 86 | 87 | def centralize_pose(self): 88 | try: 89 | (tool_trans_base, tool_rot_base) = self.tf_listener.lookupTransform(self.tip_link, self.base_link, GenerateHandEyePointState._node.get_clock().now()) 90 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 91 | Logger.logwarn('lookupTransform for robot failed!, ' + self.tip_link + ', ' + self.base_link) 92 | return 93 | 94 | tool_base_cen_m = self.tf_listener.fromTranslationRotation((tool_trans_base[0]+self.cen_delta_x, 95 | tool_trans_base[1]+self.cen_delta_y, 96 | tool_trans_base[2]+self.cen_delta_z,) 97 | ,(tool_rot_base[0], 98 | tool_rot_base[1], 99 | tool_rot_base[2], 100 | tool_rot_base[3])) 101 | 102 | base_move = np.linalg.inv(tool_base_cen_m) 103 | 104 | base_move_pos = np.array(base_move[0:3, 3]).reshape(-1) 105 | 106 | base_move_pos[0] = (self.wpose.position.x - base_move_pos[0]) 107 | base_move_pos[1] = (self.wpose.position.y - base_move_pos[1]) 108 | base_move_pos[2] = (self.wpose.position.z - base_move_pos[2]) 109 | 110 | print("delta_pos",base_move_pos) 111 | input() 112 | 113 | return base_move_pos 114 | 115 | 116 | 117 | def generate_points(self, roll_detla, pitch_delta, yaw_delta, x_distance, y_distance, z_distance): 118 | 119 | print(x_distance) 120 | # input() 121 | 122 | try: 123 | (tool_trans_base, tool_rot_base) = self.tf_listener.lookupTransform(self.tip_link, self.base_link, self.get_clock().now()) 124 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 125 | Logger.logwarn('lookupTransform for robot failed!, ' + self.tip_link + ', ' + self.base_link) 126 | return 127 | 128 | # tool_rotation = list(euler_from_quaternion([tool_rot_base[0],tool_rot_base[1],tool_rot_base[2],tool_rot_base[3]])) 129 | 130 | # quaternion = quaternion_from_euler(tool_rotation[0]+np.radians(roll_detla), 131 | # tool_rotation[1]+np.radians(pitch_delta), 132 | # tool_rotation[2]+np.radians(yaw_delta)) 133 | tool_base_m = self.tf_listener.fromTranslationRotation((tool_trans_base[0]+x_distance, 134 | tool_trans_base[1]+y_distance, 135 | tool_trans_base[2]+z_distance,) 136 | ,(tool_rot_base[0], 137 | tool_rot_base[1], 138 | tool_rot_base[2], 139 | tool_rot_base[3])) 140 | 141 | base_move_delta = np.linalg.inv(tool_base_m) 142 | 143 | delta_pos = np.array(base_move_delta[0:3, 3]).reshape(-1) 144 | 145 | 146 | delta_pos[0] = (self.wpose.position.x - delta_pos[0]) 147 | delta_pos[1] = (self.wpose.position.y - delta_pos[1]) 148 | delta_pos[2] = (self.wpose.position.z - delta_pos[2]) 149 | 150 | self.wpose.position.x += delta_pos[0] 151 | self.wpose.position.y += delta_pos[1] 152 | self.wpose.position.z += delta_pos[2] 153 | 154 | # print("delta_pos",delta_pos) 155 | 156 | # return delta_pos 157 | return 158 | 159 | 160 | 161 | 162 | def on_start(self): 163 | pass 164 | 165 | def execute(self, userdata): 166 | ''' 167 | Execute this state 168 | ''' 169 | 170 | if self.move_distance <= 0.11: 171 | quaternion = quaternion_from_euler(np.radians(self.roll), 172 | np.radians(self.pitch), 173 | np.radians(self.yaw)) 174 | if not self.rotation_correct: 175 | self.wpose.orientation.x, self.wpose.orientation.y, self.wpose.orientation.z, self.wpose.orientation.w = (quaternion[0], 176 | quaternion[1], 177 | quaternion[2], 178 | quaternion[3]) 179 | self.waypoints.append(copy.deepcopy(self.wpose)) 180 | (plan, fraction) = self._move_group.compute_cartesian_path(self.waypoints, # waypoints to follow 181 | 0.01, # eef_step 182 | 0.0) # jump_threshold 183 | self.plan_path.append(plan) 184 | userdata.hand_eye_points = self.plan_path 185 | userdata.motion_state = 'correct_rotation' 186 | self.rotation_correct = True 187 | return 'correct_pose' 188 | 189 | self.waypoints.clear() 190 | self.plan_path.clear() 191 | ##############need to decide endeffector axis################################# 192 | if self.eye_in_hand_mode: 193 | #####move axis from camera 194 | if not self.recheck: 195 | self.recheck = True 196 | userdata.result_compute = True 197 | return 'recheck' 198 | self.cen_delta_x = 0.1000-userdata.camera_h_charuco.transforms[0].translation.y 199 | self.cen_delta_y = -(0.1425-userdata.camera_h_charuco.transforms[0].translation.x) 200 | self.cen_delta_z = -(0.55-userdata.camera_h_charuco.transforms[0].translation.z) 201 | self.degree_direction = 1 202 | print(self.cen_delta_x, self.cen_delta_y, self.cen_delta_z) 203 | else: 204 | #####move axis from board 205 | if not self.recheck: 206 | self.recheck = True 207 | userdata.result_compute = True 208 | return 'recheck' 209 | self.cen_delta_x = 0.1000-userdata.camera_h_charuco.transforms[0].translation.x 210 | self.cen_delta_y = -(0.1425-userdata.camera_h_charuco.transforms[0].translation.y) 211 | self.cen_delta_z = 0.55-userdata.camera_h_charuco.transforms[0].translation.z 212 | self.degree_direction = -1 213 | # print(self.cen_delta_x, self.cen_delta_y, self.cen_delta_z) 214 | if not self.centralized: 215 | self.wpose = self._move_group.get_current_pose().pose 216 | cen_pose = self.centralize_pose() 217 | self.wpose.position.x += cen_pose[0] 218 | self.wpose.position.y += cen_pose[1] 219 | self.wpose.position.z += cen_pose[2] 220 | correct_degree = euler_from_quaternion([self.wpose.orientation.x, self.wpose.orientation.y, self.wpose.orientation.z, self.wpose.orientation.w]) 221 | self.correct_degree.append(correct_degree[0]/3.14*180.0) 222 | self.correct_degree.append(correct_degree[1]/3.14*180.0) 223 | self.correct_degree.append(correct_degree[2]/3.14*180.0) 224 | # print("self.wpose",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z) 225 | self.waypoints.append(copy.deepcopy(self.wpose)) 226 | (plan, fraction) = self._move_group.compute_cartesian_path(self.waypoints, # waypoints to follow 227 | 0.01, # eef_step 228 | 0.0) # jump_threshold 229 | self.plan_path.append(plan) 230 | userdata.hand_eye_points = self.plan_path 231 | userdata.motion_state = 'centralize' 232 | self.centralized = True 233 | return 'correct_pose' 234 | 235 | self.waypoints.clear() 236 | self.plan_path.clear() 237 | 238 | 239 | quaternion = quaternion_from_euler(np.radians(self.correct_degree[0]+15*self.degree_direction), 240 | np.radians(self.correct_degree[1]), 241 | np.radians(self.correct_degree[2])) 242 | ##############################################roll_angle, ##########################pitch_angle, ######################yaw_angle 243 | for self.loop_num in range(self.loop_size): 244 | self.wpose = self._move_group.get_current_pose().pose 245 | delta_distance = -self.move_distance+self.loop_num*(self.move_distance*2/self.loop_size) 246 | self.generate_points(15*self.degree_direction,0 ,0, delta_distance, self.move_distance, 0) 247 | self.wpose.orientation.x, self.wpose.orientation.y, self.wpose.orientation.z, self.wpose.orientation.w = (quaternion[0], 248 | quaternion[1], 249 | quaternion[2], 250 | quaternion[3]) 251 | self.waypoints.append(copy.deepcopy(self.wpose)) 252 | 253 | (plan, fraction) = self._move_group.compute_cartesian_path(self.waypoints, # waypoints to follow 254 | 0.01, # eef_step 255 | 0.0) # jump_threshold 256 | self.plan_path.append(plan) 257 | self.waypoints.clear() 258 | self.loop_num += 1 259 | quaternion = quaternion_from_euler(np.radians(self.correct_degree[0]), 260 | np.radians(self.correct_degree[1]+15*self.degree_direction), 261 | np.radians(self.correct_degree[2])) 262 | ##############################################roll_angle, ##########################pitch_angle, ######################yaw_angle 263 | 264 | for self.loop_num in range(self.loop_size,self.loop_size*2): 265 | self.wpose = self._move_group.get_current_pose().pose 266 | delta_distance = self.move_distance-(self.loop_num-self.loop_size)*(self.move_distance*2/self.loop_size) 267 | self.generate_points(0,15*self.degree_direction ,0 , self.move_distance, delta_distance, 0) 268 | self.wpose.orientation.x, self.wpose.orientation.y, self.wpose.orientation.z, self.wpose.orientation.w = (quaternion[0], 269 | quaternion[1], 270 | quaternion[2], 271 | quaternion[3]) 272 | self.waypoints.append(copy.deepcopy(self.wpose)) 273 | (plan, fraction) = self._move_group.compute_cartesian_path(self.waypoints, # waypoints to follow 274 | 0.01, # eef_step 275 | 0.0) # jump_threshold 276 | self.plan_path.append(plan) 277 | self.waypoints.clear() 278 | self.loop_num += 1 279 | quaternion = quaternion_from_euler(np.radians(self.correct_degree[0]-15*self.degree_direction), 280 | np.radians(self.correct_degree[1]), 281 | np.radians(self.correct_degree[2])) 282 | ##############################################roll_angle, ##########################pitch_angle, ######################yaw_angle 283 | for self.loop_num in range(self.loop_size*2,self.loop_size*3): 284 | self.wpose = self._move_group.get_current_pose().pose 285 | delta_distance = self.move_distance-(self.loop_num-self.loop_size*2)*(self.move_distance*2/self.loop_size) 286 | self.generate_points(-15*self.degree_direction,0 ,0, delta_distance, -self.move_distance, 0) 287 | self.wpose.orientation.x, self.wpose.orientation.y, self.wpose.orientation.z, self.wpose.orientation.w = (quaternion[0], 288 | quaternion[1], 289 | quaternion[2], 290 | quaternion[3]) 291 | self.waypoints.append(copy.deepcopy(self.wpose)) 292 | (plan, fraction) = self._move_group.compute_cartesian_path(self.waypoints, # waypoints to follow 293 | 0.01, # eef_step 294 | 0.0) # jump_threshold 295 | self.plan_path.append(plan) 296 | self.waypoints.clear() 297 | self.loop_num += 1 298 | quaternion = quaternion_from_euler(np.radians(self.correct_degree[0]), 299 | np.radians(self.correct_degree[1]-15*self.degree_direction), 300 | np.radians(self.correct_degree[2])) 301 | ##############################################roll_angle, ##########################pitch_angle, ######################yaw_angle 302 | for self.loop_num in range(self.loop_size*3,self.points_num): 303 | self.wpose = self._move_group.get_current_pose().pose 304 | delta_distance = -self.move_distance+(self.loop_num-self.loop_size*3)*(self.move_distance*2/self.loop_size) 305 | self.generate_points(0,-15*self.degree_direction ,0 , -self.move_distance, delta_distance, 0) 306 | self.wpose.orientation.x, self.wpose.orientation.y, self.wpose.orientation.z, self.wpose.orientation.w = (quaternion[0], 307 | quaternion[1], 308 | quaternion[2], 309 | quaternion[3]) 310 | self.waypoints.append(copy.deepcopy(self.wpose)) 311 | (plan, fraction) = self._move_group.compute_cartesian_path(self.waypoints, # waypoints to follow 312 | 0.01, # eef_step 313 | 0.0) # jump_threshold 314 | self.plan_path.append(plan) 315 | self.waypoints.clear() 316 | self.loop_num += 1 317 | 318 | # self.plan_path.append(self.plan_path[0]) 319 | userdata.hand_eye_points = self.plan_path 320 | userdata.motion_state = 'start_calibration' 321 | return 'done' 322 | 323 | def on_enter(self, userdata): 324 | 325 | origindegree = euler_from_quaternion([self._first_pose.pose.orientation.x, self._first_pose.pose.orientation.y, self._first_pose.pose.orientation.z, self._first_pose.pose.orientation.w]) 326 | self._origin_euler[0] = origindegree[0]/3.14*180.0 327 | self._origin_euler[1] = origindegree[1]/3.14*180.0 328 | self._origin_euler[2] = origindegree[2]/3.14*180.0 329 | 330 | # print(self.move_distance) 331 | # print("=====================================================================") 332 | 333 | # print("first aruco") 334 | print(userdata.camera_h_charuco.transforms[0].translation.x) 335 | print(userdata.camera_h_charuco.transforms[0].translation.y) 336 | print(userdata.camera_h_charuco.transforms[0].translation.z) 337 | aruco_rotation = list(euler_from_quaternion([userdata.camera_h_charuco.transforms[0].rotation.x, 338 | userdata.camera_h_charuco.transforms[0].rotation.y, 339 | userdata.camera_h_charuco.transforms[0].rotation.z, 340 | userdata.camera_h_charuco.transforms[0].rotation.w])) 341 | aruco_rotation[0] = aruco_rotation[0]/3.14*180 342 | aruco_rotation[1] = aruco_rotation[1]/3.14*180 343 | aruco_rotation[2] = aruco_rotation[2]/3.14*180 344 | # print("===============================================================") 345 | # print(aruco_rotation) 346 | # print("++++++++++++++++++++++++++++++++++++++++++++++++++++++++") 347 | 348 | 349 | if self._base_end_axis == 'xyz': 350 | self.roll = self._origin_euler[0]+1*(180+aruco_rotation[0]) 351 | self.pitch = self._origin_euler[1]+1*(0+aruco_rotation[1]) 352 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 353 | pass 354 | elif self._base_end_axis == 'xy-z': 355 | self.roll = self._origin_euler[0]+1*(180+aruco_rotation[0]) 356 | self.pitch = self._origin_euler[1]+1*(0+aruco_rotation[1]) 357 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 358 | pass 359 | elif self._base_end_axis == 'x-yz': 360 | self.roll = self._origin_euler[0]+1*(180+aruco_rotation[0]) 361 | self.pitch = self._origin_euler[1]+-1*(0+aruco_rotation[1]) 362 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 363 | pass 364 | elif self._base_end_axis == 'x-y-z': 365 | self.roll = self._origin_euler[0]+1*(180+aruco_rotation[0]) 366 | self.pitch = self._origin_euler[1]+-1*(0+aruco_rotation[1]) 367 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 368 | pass 369 | elif self._base_end_axis == '-xyz': 370 | self.roll = self._origin_euler[0]+-1*(180+aruco_rotation[0]) 371 | self.pitch = self._origin_euler[1]+1*(0+aruco_rotation[1]) 372 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 373 | pass 374 | elif self._base_end_axis == '-xy-z': 375 | self.roll = self._origin_euler[0]+-1*(180+aruco_rotation[0]) 376 | self.pitch = self._origin_euler[1]+1*(0+aruco_rotation[1]) 377 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 378 | pass 379 | elif self._base_end_axis == '-x-yz': 380 | self.roll = self._origin_euler[0]+-1*(180+aruco_rotation[0]) 381 | self.pitch = self._origin_euler[1]+-1*(0+aruco_rotation[1]) 382 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 383 | pass 384 | elif self._base_end_axis == '-x-y-z': 385 | self.roll = self._origin_euler[0]+-1*(180+aruco_rotation[0]) 386 | self.pitch = self._origin_euler[1]+-1*(0+aruco_rotation[1]) 387 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 388 | pass 389 | elif self._base_end_axis == 'yxz': 390 | self.roll = self._origin_euler[0]+1*(0+aruco_rotation[1]) 391 | self.pitch = self._origin_euler[1]+1*(180+aruco_rotation[0]) 392 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 393 | pass 394 | elif self._base_end_axis == 'yx-z': 395 | self.roll = self._origin_euler[0]+1*(0+aruco_rotation[1]) 396 | self.pitch = self._origin_euler[1]+1*(180+aruco_rotation[0]) 397 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 398 | pass 399 | elif self._base_end_axis == 'y-xz': 400 | self.roll = self._origin_euler[0]+1*(0+aruco_rotation[1]) 401 | self.pitch = self._origin_euler[1]+-1*(180+aruco_rotation[0]) 402 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 403 | pass 404 | elif self._base_end_axis == 'y-x-z': 405 | self.roll = self._origin_euler[0]+1*(0+aruco_rotation[1]) 406 | self.pitch = self._origin_euler[1]+-1*(180+aruco_rotation[0]) 407 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 408 | pass 409 | elif self._base_end_axis == '-yxz': 410 | self.roll = self._origin_euler[0]+-1*(0+aruco_rotation[1]) 411 | self.pitch = self._origin_euler[1]+1*(180+aruco_rotation[0]) 412 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 413 | pass 414 | elif self._base_end_axis == '-yx-z': 415 | self.roll = self._origin_euler[0]+-1*(0+aruco_rotation[1]) 416 | self.pitch = self._origin_euler[1]+1*(180+aruco_rotation[0]) 417 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 418 | pass 419 | elif self._base_end_axis == '-y-xz': 420 | self.roll = self._origin_euler[0]+-1*(0+aruco_rotation[1]) 421 | self.pitch = self._origin_euler[1]+-1*(180+aruco_rotation[0]) 422 | self.yaw = self._origin_euler[2]+1*(90+aruco_rotation[2]) 423 | pass 424 | elif self._base_end_axis == '-y-x-z': 425 | self.roll = self._origin_euler[0]+-1*(0+aruco_rotation[1]) 426 | self.pitch = self._origin_euler[1]+-1*(180+aruco_rotation[0]) 427 | self.yaw = self._origin_euler[2]+-1*(90+aruco_rotation[2]) 428 | pass 429 | ##################eye in hand is x-y-z cam to base(axis) 430 | #################eye to hand is -yxz board to base(axis) 431 | print("=====================================================================") 432 | 433 | pass 434 | 435 | def on_stop(self): 436 | pass 437 | 438 | def on_pause(self): 439 | pass 440 | 441 | def on_resume(self, userdata): 442 | # self.on_enter(userdata) 443 | pass 444 | --------------------------------------------------------------------------------