|
|
|
@ -0,0 +1,820 @@ |
|
|
|
#!/usr/bin/env python |
|
|
|
|
|
|
|
from __future__ import print_function |
|
|
|
|
|
|
|
from sensor_msgs.msg import JointState |
|
|
|
from geometry_msgs.msg import Twist |
|
|
|
from nav_msgs.msg import Odometry |
|
|
|
|
|
|
|
import rospy |
|
|
|
import actionlib |
|
|
|
from control_msgs.msg import FollowJointTrajectoryAction |
|
|
|
from control_msgs.msg import FollowJointTrajectoryGoal |
|
|
|
from trajectory_msgs.msg import JointTrajectoryPoint |
|
|
|
|
|
|
|
import message_filters |
|
|
|
|
|
|
|
from visualization_msgs.msg import Marker |
|
|
|
from visualization_msgs.msg import MarkerArray |
|
|
|
|
|
|
|
from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_from_matrix |
|
|
|
|
|
|
|
from sensor_msgs.msg import Imu |
|
|
|
|
|
|
|
import math |
|
|
|
import time |
|
|
|
import threading |
|
|
|
import sys |
|
|
|
|
|
|
|
import argparse as ap |
|
|
|
|
|
|
|
import numpy as np |
|
|
|
import ros_numpy |
|
|
|
|
|
|
|
from copy import deepcopy |
|
|
|
|
|
|
|
import yaml |
|
|
|
import time |
|
|
|
|
|
|
|
class CollectHeadCalibrationDataNode: |
|
|
|
|
|
|
|
def __init__(self,test_rig=False): |
|
|
|
|
|
|
|
self.rate = 10.0 |
|
|
|
|
|
|
|
self.joint_state = None |
|
|
|
self.acceleration = None |
|
|
|
|
|
|
|
self.data_time = None |
|
|
|
self.marker_time = None |
|
|
|
self.test_rig = test_rig |
|
|
|
self.data_lock = threading.Lock() |
|
|
|
if test_rig: |
|
|
|
self.number_of_testrig_samples=40 |
|
|
|
|
|
|
|
def calibration_data_callback(self, accel, marker_array, joint_state=None): |
|
|
|
# Prepare and assign data for calibration to member |
|
|
|
# variables. The configuration of the robot's joints, the |
|
|
|
# D435i acceleration, and the 3D fiducial estimates made via |
|
|
|
# D435i imagery are expected to be synchronized for this |
|
|
|
# callback. |
|
|
|
with self.data_lock: |
|
|
|
if not self.test_rig: |
|
|
|
self.data_time = joint_state.header.stamp |
|
|
|
self.joint_state = joint_state |
|
|
|
self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y, accel.linear_acceleration.z] |
|
|
|
|
|
|
|
self.wrist_inside_marker_pose = None |
|
|
|
self.wrist_top_marker_pose = None |
|
|
|
self.base_left_marker_pose = None |
|
|
|
self.base_right_marker_pose = None |
|
|
|
self.shoulder_marker_pose = None |
|
|
|
|
|
|
|
self.marker_time = None |
|
|
|
for marker in marker_array.markers: |
|
|
|
# set marker_time to the earliest marker time |
|
|
|
if self.marker_time is None: |
|
|
|
self.marker_time = marker.header.stamp |
|
|
|
elif marker.header.stamp < self.marker_time: |
|
|
|
self.marker_time = marker.header.stamp |
|
|
|
|
|
|
|
if marker.id == self.wrist_inside_marker_id: |
|
|
|
self.wrist_inside_marker_pose = marker.pose |
|
|
|
|
|
|
|
if marker.id == self.wrist_top_marker_id: |
|
|
|
self.wrist_top_marker_pose = marker.pose |
|
|
|
|
|
|
|
if marker.id == self.base_left_marker_id: |
|
|
|
self.base_left_marker_pose = marker.pose |
|
|
|
|
|
|
|
if marker.id == self.base_right_marker_id: |
|
|
|
self.base_right_marker_pose = marker.pose |
|
|
|
|
|
|
|
if marker.id == self.shoulder_marker_id: |
|
|
|
self.shoulder_marker_pose = marker.pose |
|
|
|
|
|
|
|
def move_to_pose(self, pose): |
|
|
|
# Prepare and send a goal pose to which the robot should move. |
|
|
|
joint_names = [key for key in pose] |
|
|
|
self.trajectory_goal.trajectory.joint_names = joint_names |
|
|
|
joint_positions = [pose[key] for key in joint_names] |
|
|
|
self.point.positions = joint_positions |
|
|
|
self.trajectory_goal.trajectory.points = [self.point] |
|
|
|
self.trajectory_goal.trajectory.header.stamp = rospy.Time.now() |
|
|
|
self.trajectory_client.send_goal(self.trajectory_goal) |
|
|
|
self.trajectory_client.wait_for_result() |
|
|
|
|
|
|
|
def get_samples(self, pan_angle_center, tilt_angle_center, wrist_extension_center, number_of_samples, first_move=False): |
|
|
|
# Collect N observations (samples) centered around the |
|
|
|
# provided head and wrist extension pose. If first_move is |
|
|
|
# true, it indicates that this is the first movement to a new |
|
|
|
# region for calibration, which can induce more vibrations and |
|
|
|
# thus benefit from more time to settle prior to collecting a |
|
|
|
# sample. |
|
|
|
|
|
|
|
head_motion_settle_time = 1.0 #5.0 |
|
|
|
first_pan_tilt_extra_settle_time = 1.0 |
|
|
|
wait_before_each_sample_s = 0.2 |
|
|
|
|
|
|
|
# generate pan and wrist backlash samples |
|
|
|
|
|
|
|
# tilt backlash |
|
|
|
tilt_backlash_transition = self.tilt_angle_backlash_transition_rad |
|
|
|
# Ensure that the head is clearly in one of the two head tilt |
|
|
|
# backlash regimes and not in the indeterminate transition |
|
|
|
# region. |
|
|
|
tilt_backlash_safety_margin = 0.08 |
|
|
|
tilt_backlash_looking_down_max = tilt_backlash_transition - tilt_backlash_safety_margin # looking down |
|
|
|
tilt_backlash_looking_up_min = tilt_backlash_transition + tilt_backlash_safety_margin # looking up |
|
|
|
|
|
|
|
tilt_angle = tilt_angle_center |
|
|
|
if (tilt_angle_center > tilt_backlash_looking_down_max) and (tilt_angle_center < tilt_backlash_looking_up_min): |
|
|
|
# Commanded tilt_angle_center is in an uncertain backlash range |
|
|
|
# of tilt, so change it to be in a confident range. |
|
|
|
down_diff = abs(tilt_backlash_looking_down_max - tilt_angle_center) |
|
|
|
up_diff = abs(tilt_backlash_looking_up_min - tilt_angle_center) |
|
|
|
if up_diff < down_diff: |
|
|
|
tilt_angle = tilt_backlash_looking_up_min |
|
|
|
else: |
|
|
|
tilt_angle = tilt_backlash_looking_down_max |
|
|
|
if tilt_angle > tilt_backlash_transition: |
|
|
|
joint_head_tilt_looking_up = True |
|
|
|
else: |
|
|
|
joint_head_tilt_looking_up = False |
|
|
|
|
|
|
|
# represent all head pan and wrist extension backlash states |
|
|
|
backlash_combinations = ((False, False), (False, True), (True, True), (True, False)) |
|
|
|
|
|
|
|
pan_backlash_change_rad = 0.07 # ~8 degrees |
|
|
|
wrist_backlash_change_m = 0.01 # 1 cm |
|
|
|
|
|
|
|
samples = [] |
|
|
|
|
|
|
|
# collect samples for all head pan and wrist extension |
|
|
|
# backlash states |
|
|
|
for joint_head_pan_looked_left, wrist_extension_retracted in backlash_combinations: |
|
|
|
|
|
|
|
if joint_head_pan_looked_left: |
|
|
|
pan_angle = pan_angle_center + pan_backlash_change_rad |
|
|
|
else: |
|
|
|
pan_angle = pan_angle_center - pan_backlash_change_rad |
|
|
|
|
|
|
|
if wrist_extension_retracted: |
|
|
|
wrist_extension = wrist_extension_center - wrist_backlash_change_m |
|
|
|
else: |
|
|
|
wrist_extension = wrist_extension_center + wrist_backlash_change_m |
|
|
|
|
|
|
|
# move to backlash state |
|
|
|
head_arm_pose = {'joint_head_pan': pan_angle, |
|
|
|
'joint_head_tilt': tilt_angle, |
|
|
|
'wrist_extension': wrist_extension} |
|
|
|
self.move_to_pose(head_arm_pose) |
|
|
|
|
|
|
|
|
|
|
|
# wait for the joints and sensors to settle |
|
|
|
rospy.sleep(head_motion_settle_time) |
|
|
|
settled_time = rospy.Time.now() |
|
|
|
|
|
|
|
# The first move to a pose is typically larger and can |
|
|
|
# benefit from more settling time due to induced |
|
|
|
# vibrations. |
|
|
|
if first_move: |
|
|
|
rospy.sleep(first_pan_tilt_extra_settle_time) |
|
|
|
|
|
|
|
for sample_number in range(number_of_samples): |
|
|
|
rospy.sleep(wait_before_each_sample_s) |
|
|
|
observation = {'joints': {'joint_head_pan': None, |
|
|
|
'joint_head_tilt': None, |
|
|
|
'wrist_extension' : None, |
|
|
|
'joint_lift': None }, |
|
|
|
'backlash_state': { |
|
|
|
'joint_head_pan_looked_left': None, |
|
|
|
'joint_head_tilt_looking_up': None, |
|
|
|
'wrist_extension_retracted': None |
|
|
|
}, |
|
|
|
'camera_measurements': { 'd435i_acceleration': None, |
|
|
|
'wrist_inside_marker_pose': None, |
|
|
|
'wrist_top_marker_pose': None, |
|
|
|
'base_left_marker_pose': None, |
|
|
|
'base_right_marker_pose': None, |
|
|
|
'shoulder_marker_pose': None} |
|
|
|
} |
|
|
|
|
|
|
|
# wait for a sample taken after the robot has |
|
|
|
# settled |
|
|
|
timeout_duration = rospy.Duration(secs=10) |
|
|
|
start_wait = rospy.Time.now() |
|
|
|
timeout = False |
|
|
|
data_ready = False |
|
|
|
while (not data_ready) and (not timeout): |
|
|
|
# check the timing of the current sample |
|
|
|
with self.data_lock: |
|
|
|
if (self.data_time is not None) and (self.data_time > settled_time): |
|
|
|
if (self.marker_time is None): |
|
|
|
# joint_states and acceleration |
|
|
|
# were taken after the robot |
|
|
|
# settled and there are no aruco |
|
|
|
# marker poses |
|
|
|
data_ready = True |
|
|
|
elif (self.marker_time > settled_time): |
|
|
|
# joint_states, acceleration, |
|
|
|
# and aruco marker poses were |
|
|
|
# taken after the robot |
|
|
|
# settled |
|
|
|
data_ready = True |
|
|
|
if not data_ready: |
|
|
|
#rospy.sleep(0.2) #0.1 |
|
|
|
rospy.sleep(1.0) |
|
|
|
timeout = (rospy.Time.now() - start_wait) > timeout_duration |
|
|
|
|
|
|
|
if timeout: |
|
|
|
rospy.logerr('collect_head_calibration_data get_samples: timeout while waiting for sample.') |
|
|
|
raise Exception('Timed out waiting for joint_states/accelerations/markers messages after 10 seconds') |
|
|
|
|
|
|
|
with self.data_lock: |
|
|
|
#set backlash joint state |
|
|
|
backlash_state = observation['backlash_state'] |
|
|
|
backlash_state['joint_head_pan_looked_left'] = joint_head_pan_looked_left |
|
|
|
backlash_state['joint_head_tilt_looking_up'] = joint_head_tilt_looking_up |
|
|
|
backlash_state['wrist_extension_retracted'] = wrist_extension_retracted |
|
|
|
|
|
|
|
# record the settled joint values |
|
|
|
joints = observation['joints'] |
|
|
|
for joint_name in joints: |
|
|
|
print('Joint name %s'%joint_name) |
|
|
|
joint_index = self.joint_state.name.index(joint_name) |
|
|
|
joint_value = self.joint_state.position[joint_index] |
|
|
|
joints[joint_name] = joint_value |
|
|
|
|
|
|
|
# record the settled camera measurments |
|
|
|
camera_measurements = observation['camera_measurements'] |
|
|
|
camera_measurements['d435i_acceleration'] = self.acceleration |
|
|
|
|
|
|
|
# record the settled aruco measurements |
|
|
|
if self.wrist_inside_marker_pose is not None: |
|
|
|
camera_measurements['wrist_inside_marker_pose'] = ros_numpy.numpify(self.wrist_inside_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.wrist_top_marker_pose is not None: |
|
|
|
camera_measurements['wrist_top_marker_pose'] = ros_numpy.numpify(self.wrist_top_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.base_left_marker_pose is not None: |
|
|
|
camera_measurements['base_left_marker_pose'] = ros_numpy.numpify(self.base_left_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.base_right_marker_pose is not None: |
|
|
|
camera_measurements['base_right_marker_pose'] = ros_numpy.numpify(self.base_right_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.shoulder_marker_pose is not None: |
|
|
|
camera_measurements['shoulder_marker_pose'] = ros_numpy.numpify(self.shoulder_marker_pose).tolist() |
|
|
|
|
|
|
|
samples.append(observation) |
|
|
|
return samples |
|
|
|
|
|
|
|
def get_samples_testrig(self, number_of_samples): |
|
|
|
# Collect N observations (samples) centered around the |
|
|
|
# provided head and wrist extension pose. If first_move is |
|
|
|
# true, it indicates that this is the first movement to a new |
|
|
|
# region for calibration, which can induce more vibrations and |
|
|
|
# thus benefit from more time to settle prior to collecting a |
|
|
|
# sample. |
|
|
|
|
|
|
|
samples = [] |
|
|
|
wait_before_each_sample_s = 0.2 |
|
|
|
for sample_number in range(number_of_samples): |
|
|
|
rospy.sleep(wait_before_each_sample_s) |
|
|
|
observation = {'joints': {'joint_head_pan': None, |
|
|
|
'joint_head_tilt': None, |
|
|
|
'wrist_extension' : None, |
|
|
|
'joint_lift': None }, |
|
|
|
'backlash_state': { |
|
|
|
'joint_head_pan_looked_left': None, |
|
|
|
'joint_head_tilt_looking_up': None, |
|
|
|
'wrist_extension_retracted': None |
|
|
|
}, |
|
|
|
'camera_measurements': { 'd435i_acceleration': None, |
|
|
|
'wrist_inside_marker_pose': None, |
|
|
|
'wrist_top_marker_pose': None, |
|
|
|
'base_left_marker_pose': None, |
|
|
|
'base_right_marker_pose': None, |
|
|
|
'shoulder_marker_pose': None} |
|
|
|
} |
|
|
|
|
|
|
|
# wait for a sample taken after the robot has |
|
|
|
# settled |
|
|
|
# timeout_duration = rospy.Duration(secs=10) |
|
|
|
# start_wait = rospy.Time.now() |
|
|
|
# timeout = False |
|
|
|
# data_ready = False |
|
|
|
# while (not data_ready) and (not timeout): |
|
|
|
# # check the timing of the current sample |
|
|
|
# with self.data_lock: |
|
|
|
# if (self.data_time is not None) and (self.data_time > settled_time): |
|
|
|
# if (self.marker_time is None): |
|
|
|
# # joint_states and acceleration |
|
|
|
# # were taken after the robot |
|
|
|
# # settled and there are no aruco |
|
|
|
# # marker poses |
|
|
|
# data_ready = True |
|
|
|
# elif (self.marker_time > settled_time): |
|
|
|
# # joint_states, acceleration, |
|
|
|
# # and aruco marker poses were |
|
|
|
# # taken after the robot |
|
|
|
# # settled |
|
|
|
# data_ready = True |
|
|
|
# if not data_ready: |
|
|
|
# #rospy.sleep(0.2) #0.1 |
|
|
|
# rospy.sleep(1.0) |
|
|
|
# timeout = (rospy.Time.now() - start_wait) > timeout_duration |
|
|
|
|
|
|
|
# if timeout: |
|
|
|
# rospy.logerr('collect_head_calibration_data get_samples: timeout while waiting for sample.') |
|
|
|
# raise Exception('Timed out waiting for joint_states/accelerations/markers messages after 10 seconds') |
|
|
|
|
|
|
|
with self.data_lock: |
|
|
|
|
|
|
|
# record the settled camera measurments |
|
|
|
camera_measurements = observation['camera_measurements'] |
|
|
|
camera_measurements['d435i_acceleration'] = self.acceleration |
|
|
|
|
|
|
|
# record the settled aruco measurements |
|
|
|
if self.wrist_inside_marker_pose is not None: |
|
|
|
camera_measurements['wrist_inside_marker_pose'] = ros_numpy.numpify(self.wrist_inside_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.wrist_top_marker_pose is not None: |
|
|
|
camera_measurements['wrist_top_marker_pose'] = ros_numpy.numpify(self.wrist_top_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.base_left_marker_pose is not None: |
|
|
|
camera_measurements['base_left_marker_pose'] = ros_numpy.numpify(self.base_left_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.base_right_marker_pose is not None: |
|
|
|
camera_measurements['base_right_marker_pose'] = ros_numpy.numpify(self.base_right_marker_pose).tolist() |
|
|
|
|
|
|
|
if self.shoulder_marker_pose is not None: |
|
|
|
camera_measurements['shoulder_marker_pose'] = ros_numpy.numpify(self.shoulder_marker_pose).tolist() |
|
|
|
|
|
|
|
samples.append(observation) |
|
|
|
return samples |
|
|
|
|
|
|
|
def calibrate_pan_and_tilt(self, collect_check_data=False): |
|
|
|
# Collects observations of fiducial markers on the robot's |
|
|
|
# body while moving the head pan, head tilt, arm lift, and arm |
|
|
|
# extension. There are five markers in total with the |
|
|
|
# following locations: front left of the mobile base, front |
|
|
|
# right of the mobile base, top of the shoulder, top of the |
|
|
|
# wrist, and inside the wrist. |
|
|
|
# |
|
|
|
# When collect_check_data is True, fewer samples are collected |
|
|
|
# with the intention of using them to check the current |
|
|
|
# calibration. When collect_check_data is False, more samples |
|
|
|
# are collected with the intention of fully calibrating the |
|
|
|
# robot. |
|
|
|
|
|
|
|
calibration_data = [] |
|
|
|
number_of_samples_per_head_pose = 1 #3 |
|
|
|
wrist_motion_settle_time = 1.0 |
|
|
|
|
|
|
|
######################################## |
|
|
|
## |
|
|
|
## COLLECT GLOBAL HEAD LOOKING DOWN DATA |
|
|
|
## |
|
|
|
######################################## |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('COLLECT GLOBAL HEAD LOOKING DOWN DATA') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
# looking down data |
|
|
|
min_tilt_angle_rad = -1.4 |
|
|
|
max_tilt_angle_rad = -0.5 |
|
|
|
|
|
|
|
min_pan_angle_rad = -3.8 |
|
|
|
max_pan_angle_rad = 1.3 |
|
|
|
|
|
|
|
if collect_check_data: |
|
|
|
number_of_tilt_steps = 1 |
|
|
|
number_of_pan_steps = 3 |
|
|
|
else: |
|
|
|
number_of_tilt_steps = 3 #4 |
|
|
|
number_of_pan_steps = 5 #7 |
|
|
|
|
|
|
|
pan_angles_rad = np.linspace(min_pan_angle_rad, max_pan_angle_rad, number_of_pan_steps) |
|
|
|
tilt_angles_rad = np.linspace(min_tilt_angle_rad, max_tilt_angle_rad, number_of_tilt_steps) |
|
|
|
|
|
|
|
rospy.loginfo('Move to a new arm pose.') |
|
|
|
wrist_extension = 0.12 |
|
|
|
wrist_pose = {'wrist_extension': wrist_extension} |
|
|
|
self.move_to_pose(wrist_pose) |
|
|
|
lift_m = 0.16 |
|
|
|
lift_pose = {'joint_lift': lift_m} |
|
|
|
self.move_to_pose(lift_pose) |
|
|
|
|
|
|
|
# wait for the joints and sensors to settle |
|
|
|
rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) |
|
|
|
rospy.sleep(wrist_motion_settle_time) |
|
|
|
|
|
|
|
rospy.loginfo('Starting to collect global head looking down data (expect to collect {0} samples).'.format(number_of_tilt_steps * number_of_pan_steps)) |
|
|
|
first_pan_tilt = True |
|
|
|
for pan_angle in pan_angles_rad: |
|
|
|
for tilt_angle in tilt_angles_rad: |
|
|
|
observation = self.get_samples(pan_angle, tilt_angle, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
first_pan_tilt = False |
|
|
|
calibration_data.extend(observation) |
|
|
|
n = len(calibration_data) |
|
|
|
if (n % 10) == 0: |
|
|
|
rospy.loginfo('{0} samples collected so far.'.format(n)) |
|
|
|
|
|
|
|
####################################### |
|
|
|
## |
|
|
|
## COLLECT GLOBAL HEAD LOOKING UP DATA |
|
|
|
## |
|
|
|
####################################### |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('COLLECT GLOBAL HEAD LOOKING UP DATA') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
if not collect_check_data: |
|
|
|
tilt_angles_rad = [-0.3, 0.38] |
|
|
|
pan_angles_rad = [-3.8, -1.66, 1.33] |
|
|
|
|
|
|
|
rospy.loginfo('Move to a new arm pose.') |
|
|
|
lift_m = 0.92 |
|
|
|
lift_pose = {'joint_lift': lift_m} |
|
|
|
self.move_to_pose(lift_pose) |
|
|
|
wrist_extension = 0.29 |
|
|
|
wrist_pose = {'wrist_extension': wrist_extension} |
|
|
|
self.move_to_pose(wrist_pose) |
|
|
|
|
|
|
|
# wait for the joints and sensors to settle |
|
|
|
rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) |
|
|
|
rospy.sleep(wrist_motion_settle_time) |
|
|
|
|
|
|
|
rospy.loginfo('Starting to collect global head looking up data (expect to collect {0} samples).'.format(len(pan_angles_rad) * len(tilt_angles_rad))) |
|
|
|
first_pan_tilt = True |
|
|
|
for pan_angle in pan_angles_rad: |
|
|
|
for tilt_angle in tilt_angles_rad: |
|
|
|
observation = self.get_samples(pan_angle, tilt_angle, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
first_pan_tilt = False |
|
|
|
calibration_data.extend(observation) |
|
|
|
n = len(calibration_data) |
|
|
|
if (n % 10) == 0: |
|
|
|
rospy.loginfo('{0} samples collected so far.'.format(n)) |
|
|
|
|
|
|
|
####################################### |
|
|
|
## |
|
|
|
## COLLECT HIGH SHOULDER DATA |
|
|
|
## |
|
|
|
####################################### |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('COLLECT HIGH SHOULDER DATA') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
############################################################## |
|
|
|
### poses that see the shoulder marker |
|
|
|
# |
|
|
|
# lift 0.8 |
|
|
|
# pan -3.82 to -3.619 |
|
|
|
# tilt -0.918 to -0.49 |
|
|
|
# |
|
|
|
# lift 0.2 |
|
|
|
# pan -3.85 to -2.48 |
|
|
|
# tilt -1.23 to -0.87 |
|
|
|
# |
|
|
|
# two poses with good coverage |
|
|
|
# (needs to account for +/- 0.07 rad for pan backlash) |
|
|
|
# pan -3.7, tilt -0.7 |
|
|
|
# pan -3.7, tilt -1.1 |
|
|
|
# |
|
|
|
# Marker detected with point clouds |
|
|
|
# lift 0.5, pan -3.7, tilt = -1.4 to -1.2 |
|
|
|
# lift 0.65, pan -3.85, tilt = -1.0 |
|
|
|
# lift 0.74, pan -3.85, tilt = -0.78 to -1.4 (-1.4 also sees the base markers) |
|
|
|
############################################################## |
|
|
|
|
|
|
|
wrist_extension = 0.29 |
|
|
|
|
|
|
|
# low shoulder height |
|
|
|
pan_angle_rad = -3.7 |
|
|
|
tilt_angle_rad_1 = -1.4 |
|
|
|
tilt_angle_rad_2 = -1.2 |
|
|
|
lift_m = 0.5 |
|
|
|
|
|
|
|
pose = {'joint_lift': lift_m} |
|
|
|
rospy.loginfo('Move to first shoulder capture height.') |
|
|
|
self.move_to_pose(pose) |
|
|
|
rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) |
|
|
|
rospy.sleep(wrist_motion_settle_time) |
|
|
|
|
|
|
|
first_pan_tilt = True |
|
|
|
observation = self.get_samples(pan_angle_rad, tilt_angle_rad_1, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
calibration_data.extend(observation) |
|
|
|
|
|
|
|
if not collect_check_data: |
|
|
|
first_pan_tilt = False |
|
|
|
observation = self.get_samples(pan_angle_rad, tilt_angle_rad_2, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
calibration_data.extend(observation) |
|
|
|
|
|
|
|
# high shoulder height |
|
|
|
pan_angle_rad = -3.85 |
|
|
|
tilt_angle_rad_1 = -0.8 |
|
|
|
tilt_angle_rad_2 = -1.4 |
|
|
|
lift_m = 0.74 |
|
|
|
|
|
|
|
pose = {'joint_lift': lift_m} |
|
|
|
rospy.loginfo('Move to second shoulder capture height.') |
|
|
|
self.move_to_pose(pose) |
|
|
|
rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) |
|
|
|
rospy.sleep(wrist_motion_settle_time) |
|
|
|
|
|
|
|
first_pan_tilt = True |
|
|
|
observation = self.get_samples(pan_angle_rad, tilt_angle_rad_1, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
calibration_data.extend(observation) |
|
|
|
|
|
|
|
if not collect_check_data: |
|
|
|
first_pan_tilt = False |
|
|
|
observation = self.get_samples(pan_angle_rad, tilt_angle_rad_2, wrist_extension, number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
calibration_data.extend(observation) |
|
|
|
|
|
|
|
|
|
|
|
####################################### |
|
|
|
## |
|
|
|
## COLLECT ARM FOCUSED DATA |
|
|
|
## |
|
|
|
####################################### |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('COLLECT ARM FOCUSED DATA') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
############################################################## |
|
|
|
### poses that see both of the arm markers |
|
|
|
# |
|
|
|
# lift height 0.8 |
|
|
|
# extension 0.4 tilt -0.52 pan -2.06 to -1.45 |
|
|
|
# extension 0.03 tilt -1.27 pan -2.33 to -1.48 |
|
|
|
# |
|
|
|
# lift height 0.2 |
|
|
|
# extension 0.4 tilt -0.9 pan -2.9 to -0.93 |
|
|
|
# extension 0.03 tilt -1.23 pan -3.2 to -0.4 (full range) |
|
|
|
# |
|
|
|
# ranges with good coverage |
|
|
|
# (needs to account for +/- 0.07 rad for pan backlash) |
|
|
|
# tilt -1.2 to -0.5 |
|
|
|
# pan -2.0 to -1.45 |
|
|
|
# |
|
|
|
############################################################## |
|
|
|
|
|
|
|
wrist_focused_min_pan = -2.0 |
|
|
|
wrist_focused_max_pan = -1.45 |
|
|
|
|
|
|
|
wrist_focused_min_tilt = -1.2 |
|
|
|
wrist_focused_max_tilt = -0.5 |
|
|
|
|
|
|
|
|
|
|
|
if collect_check_data: |
|
|
|
num_wrist_focused_pan_steps = 1 #3 |
|
|
|
num_wrist_focused_tilt_steps = 1 #3 |
|
|
|
else: |
|
|
|
num_wrist_focused_pan_steps = 2 #3 |
|
|
|
num_wrist_focused_tilt_steps = 2 #3 |
|
|
|
|
|
|
|
wrist_focused_pan_angles_rad = np.linspace(wrist_focused_min_pan, wrist_focused_max_pan, num_wrist_focused_pan_steps) |
|
|
|
wrist_focused_tilt_angles_rad = np.linspace(wrist_focused_min_tilt, wrist_focused_max_tilt, num_wrist_focused_tilt_steps) |
|
|
|
|
|
|
|
if collect_check_data: |
|
|
|
wrist_extensions = [0.2] |
|
|
|
# Already have data from a single extension at a height of |
|
|
|
# 1.2m from global camera looking down, so try using only 2 |
|
|
|
# heights. |
|
|
|
# |
|
|
|
# 0.46 m = (0.8 m + 0.12 m) / 2.0 |
|
|
|
wrist_heights = [0.46, 0.8] #[0.2, 0.5, 0.8] |
|
|
|
wrist_poses = [{'wrist_extension': e, 'joint_lift': h} for h in wrist_heights for e in wrist_extensions] |
|
|
|
|
|
|
|
num_wrist_poses = len(wrist_poses) |
|
|
|
else: |
|
|
|
wrist_extensions = [0.03, 0.4] |
|
|
|
# Already have data from a single extension at a height of |
|
|
|
# 1.2m from global camera looking down, so try using only 2 |
|
|
|
# heights. |
|
|
|
# |
|
|
|
# 0.46 m = (0.8 m + 0.12 m) / 2.0 |
|
|
|
wrist_heights = [0.46, 0.8] #[0.2, 0.5, 0.8] |
|
|
|
wrist_poses = [{'wrist_extension': e, 'joint_lift': h} for h in wrist_heights for e in wrist_extensions] |
|
|
|
|
|
|
|
num_wrist_poses = len(wrist_poses) |
|
|
|
|
|
|
|
rospy.loginfo('Starting to collect arm focused samples (expect to collect {0} samples).'.format(num_wrist_poses * num_wrist_focused_pan_steps * num_wrist_focused_tilt_steps)) |
|
|
|
|
|
|
|
for wrist_pose in wrist_poses: |
|
|
|
|
|
|
|
rospy.loginfo('Move to a new arm pose.') |
|
|
|
self.move_to_pose(wrist_pose) |
|
|
|
|
|
|
|
# wait for the joints and sensors to settle |
|
|
|
rospy.loginfo('Wait {0} seconds for the robot to settle after motion of the arm.'.format(wrist_motion_settle_time)) |
|
|
|
rospy.sleep(wrist_motion_settle_time) |
|
|
|
|
|
|
|
first_pan_tilt = True |
|
|
|
for pan_angle in wrist_focused_pan_angles_rad: |
|
|
|
for tilt_angle in wrist_focused_tilt_angles_rad: |
|
|
|
observation = self.get_samples(pan_angle, tilt_angle, wrist_pose['wrist_extension'], number_of_samples_per_head_pose, first_move=first_pan_tilt) |
|
|
|
first_pan_tilt = False |
|
|
|
calibration_data.extend(observation) |
|
|
|
n = len(calibration_data) |
|
|
|
if (n % 10) == 0: |
|
|
|
rospy.loginfo('{0} samples collected so far.'.format(n)) |
|
|
|
|
|
|
|
####################################### |
|
|
|
## |
|
|
|
## FINISHED COLLECTING DATA - SAVE IT |
|
|
|
## |
|
|
|
####################################### |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('FINISHED COLLECTING DATA') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('Collected {0} samples in total.'.format(len(calibration_data))) |
|
|
|
|
|
|
|
t = time.localtime() |
|
|
|
capture_date = str(t.tm_year) + str(t.tm_mon).zfill(2) + str(t.tm_mday).zfill(2) + str(t.tm_hour).zfill(2) + str(t.tm_min).zfill(2) |
|
|
|
if collect_check_data: |
|
|
|
filename = self.calibration_directory + 'check_head_calibration_data_' + capture_date + '.yaml' |
|
|
|
else: |
|
|
|
filename = self.calibration_directory + 'head_calibration_data_' + capture_date + '.yaml' |
|
|
|
rospy.loginfo('Saving calibration_data to a YAML file named {0}'.format(filename)) |
|
|
|
fid = open(filename, 'w') |
|
|
|
yaml.dump(calibration_data, fid) |
|
|
|
fid.close() |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
####################################### |
|
|
|
|
|
|
|
def testrig_data_collect(self, number_of_samples): |
|
|
|
|
|
|
|
calibration_data = [] |
|
|
|
|
|
|
|
######################################## |
|
|
|
## |
|
|
|
## COLLECT DATA FROM TEST RIG |
|
|
|
## |
|
|
|
######################################## |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('COLLECT DATA FROM TEST RIG') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
while len(calibration_data)<=number_of_samples: |
|
|
|
observation = self.get_samples_testrig(1) |
|
|
|
first_pan_tilt = False |
|
|
|
calibration_data.extend(observation) |
|
|
|
n = len(calibration_data) |
|
|
|
if (n % 10) == 0: |
|
|
|
rospy.loginfo('{0} samples collected so far.'.format(n)) |
|
|
|
|
|
|
|
####################################### |
|
|
|
## |
|
|
|
## FINISHED COLLECTING DATA - SAVE IT |
|
|
|
## |
|
|
|
####################################### |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('FINISHED COLLECTING DATA') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('Collected {0} samples in total.'.format(len(calibration_data))) |
|
|
|
|
|
|
|
t = time.localtime() |
|
|
|
capture_date = str(t.tm_year) + str(t.tm_mon).zfill(2) + str(t.tm_mday).zfill(2) + str(t.tm_hour).zfill(2) + str(t.tm_min).zfill(2) |
|
|
|
|
|
|
|
temp_loc = '/home/hello-robot/catkin_ws/src/stretch_ros/stretch_camera_testrig/data/' #Change it |
|
|
|
filename = temp_loc + 'testrig_collected_data_' + capture_date + '.yaml' |
|
|
|
|
|
|
|
rospy.loginfo('Saving calibration_data to a YAML file named {0}'.format(filename)) |
|
|
|
fid = open(filename, 'w') |
|
|
|
yaml.dump(calibration_data, fid) |
|
|
|
fid.close() |
|
|
|
rospy.loginfo('') |
|
|
|
rospy.loginfo('*************************************') |
|
|
|
rospy.loginfo('') |
|
|
|
|
|
|
|
####################################### |
|
|
|
|
|
|
|
def move_to_initial_configuration(self): |
|
|
|
# The robot is commanded to move to this pose prior to |
|
|
|
# beginning to collect calibration data. |
|
|
|
initial_pose_map = { |
|
|
|
'joint_wrist_yaw': 0.0, |
|
|
|
'joint_wrist_pitch': 0.0, |
|
|
|
'joint_wrist_roll': 0.0, |
|
|
|
'wrist_extension': 0.0, |
|
|
|
'joint_lift': 0.3, |
|
|
|
'gripper_aperture': 0.0, |
|
|
|
'joint_head_pan': -1.6947147036864942, |
|
|
|
'joint_head_tilt': -0.4 |
|
|
|
} |
|
|
|
|
|
|
|
initial_pose = {} |
|
|
|
for j in self.joint_state.name: |
|
|
|
if j in initial_pose_map: |
|
|
|
initial_pose[j] = initial_pose_map[j] |
|
|
|
|
|
|
|
rospy.loginfo('Move to the calibration start pose.') |
|
|
|
self.move_to_pose(initial_pose) |
|
|
|
|
|
|
|
def main(self, collect_check_data): |
|
|
|
rospy.init_node('collect_head_calibration_data') |
|
|
|
self.node_name = rospy.get_name() |
|
|
|
rospy.loginfo("{0} started".format(self.node_name)) |
|
|
|
|
|
|
|
# Obtain the ArUco marker ID numbers. |
|
|
|
self.marker_info = rospy.get_param('/aruco_marker_info') |
|
|
|
for k in self.marker_info.keys(): |
|
|
|
m = self.marker_info[k] |
|
|
|
if m['link'] == 'link_aruco_left_base': |
|
|
|
self.base_left_marker_id = int(k) |
|
|
|
if m['link'] == 'link_aruco_right_base': |
|
|
|
self.base_right_marker_id = int(k) |
|
|
|
if m['link'] == 'link_aruco_inner_wrist': |
|
|
|
self.wrist_inside_marker_id = int(k) |
|
|
|
if m['link'] == 'link_aruco_top_wrist': |
|
|
|
self.wrist_top_marker_id = int(k) |
|
|
|
if m['link'] == 'link_aruco_shoulder': |
|
|
|
self.shoulder_marker_id = int(k) |
|
|
|
|
|
|
|
if not self.test_rig: |
|
|
|
filename = rospy.get_param('~controller_calibration_file') |
|
|
|
|
|
|
|
rospy.loginfo('Loading factory default tilt backlash transition angle from the YAML file named {0}'.format(filename)) |
|
|
|
fid = open(filename, 'r') |
|
|
|
controller_parameters = yaml.load(fid) |
|
|
|
fid.close() |
|
|
|
self.tilt_angle_backlash_transition_rad = controller_parameters['tilt_angle_backlash_transition'] |
|
|
|
deg_per_rad = 180.0/math.pi |
|
|
|
rospy.loginfo('self.tilt_angle_backlash_transition_rad in degrees = {0}'.format(self.tilt_angle_backlash_transition_rad * deg_per_rad)) |
|
|
|
|
|
|
|
self.calibration_directory = rospy.get_param('~calibration_directory') |
|
|
|
rospy.loginfo('Using the following directory for calibration files: {0}'.format(self.calibration_directory)) |
|
|
|
|
|
|
|
# Setup time synchronization for calibration data. |
|
|
|
joint_state_subscriber = message_filters.Subscriber('/stretch/joint_states', JointState) |
|
|
|
accel_subscriber = message_filters.Subscriber('/camera/accel/sample_corrected', Imu) |
|
|
|
aruco_subscriber = message_filters.Subscriber('/aruco/marker_array', MarkerArray) |
|
|
|
slop_time = 0.1 |
|
|
|
self.synchronizer = message_filters.ApproximateTimeSynchronizer([accel_subscriber, aruco_subscriber, joint_state_subscriber], 10, slop_time, allow_headerless=True) |
|
|
|
self.synchronizer.registerCallback(self.calibration_data_callback) |
|
|
|
|
|
|
|
self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction) |
|
|
|
self.trajectory_goal = FollowJointTrajectoryGoal() |
|
|
|
self.trajectory_goal.goal_time_tolerance = rospy.Time(1.0) |
|
|
|
|
|
|
|
self.point = JointTrajectoryPoint() |
|
|
|
self.point.time_from_start = rospy.Duration(0.0) |
|
|
|
|
|
|
|
server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0)) |
|
|
|
if not server_reached: |
|
|
|
rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') |
|
|
|
sys.exit() |
|
|
|
|
|
|
|
rate = rospy.Rate(self.rate) |
|
|
|
while self.joint_state is None: |
|
|
|
rate.sleep() |
|
|
|
self.move_to_initial_configuration() |
|
|
|
|
|
|
|
self.calibrate_pan_and_tilt(collect_check_data) |
|
|
|
|
|
|
|
else: |
|
|
|
# Setup time synchronization for collecting testrig data. |
|
|
|
accel_subscriber = message_filters.Subscriber('/camera/accel/sample_corrected', Imu) |
|
|
|
aruco_subscriber = message_filters.Subscriber('/aruco/marker_array', MarkerArray) |
|
|
|
slop_time = 0.1 |
|
|
|
self.synchronizer = message_filters.ApproximateTimeSynchronizer([accel_subscriber, aruco_subscriber], 10, slop_time, allow_headerless=True) |
|
|
|
self.synchronizer.registerCallback(self.calibration_data_callback) |
|
|
|
|
|
|
|
|
|
|
|
rate = rospy.Rate(self.rate) |
|
|
|
|
|
|
|
self.testrig_data_collect(number_of_samples=self.number_of_testrig_samples) |
|
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__': |
|
|
|
parser = ap.ArgumentParser(description='Collect head calibration data.') |
|
|
|
parser.add_argument('--check', action='store_true', help='Collect data to check the current calibration, instead of data to perform a new calibration.') |
|
|
|
|
|
|
|
args, unknown = parser.parse_known_args() |
|
|
|
collect_check_data = args.check |
|
|
|
|
|
|
|
try: |
|
|
|
node = CollectHeadCalibrationDataNode() |
|
|
|
node.main(collect_check_data) |
|
|
|
except rospy.ROSInterruptException: |
|
|
|
pass |
|
|
|
|
Mohamed Fazil
2 years ago