def setupConfig(self):
self.config = dict()
self.config['scan'] = dict()
self.config['scan']['pose_list'] = ['scan_left', 'scan_right']
self.config['scan']['joint_speed'] = 60
self.config['grasp_speed'] = 20
self.config['home_pose_name'] = 'above_table_pre_grasp'
self.config['grasp_nominal_direction'] = np.array([1, 0,
0]) # x forwards
self.config['grasp_to_ee'] = dict()
self.config['grasp_to_ee']['translation'] = dict()
self.config['grasp_to_ee']['translation']['x'] = 9.32362425e-02
self.config['grasp_to_ee']['translation']['y'] = 0
self.config['grasp_to_ee']['translation']['z'] = 0
self.config['grasp_to_ee']['orientation'] = dict()
self.config['grasp_to_ee']['orientation']['w'] = 0.97921432
self.config['grasp_to_ee']['orientation']['x'] = -0.20277454
self.config['grasp_to_ee']['orientation']['y'] = 0.00454233
self.config['grasp_to_ee']['orientation']['z'] = -0.00107904
self.graspToIiiwaLinkEE = spartanUtils.transformFromPose(
self.config['grasp_to_ee'])
self.iiwaLinkEEToGraspFrame = self.graspToIiiwaLinkEE.GetLinearInverse(
)
pos = [-0.15, 0, 0]
quat = [1, 0, 0, 0]
self.preGraspToGraspTransform = transformUtils.transformFromPose(
pos, quat)
def parseCameraInfo(self, cameraInfoYamlDict):
self.cameraInfo = dict()
for cameraType in self.cameraTypes:
d = cameraInfoYamlDict[cameraType]
data = dict()
data['raw_data'] = d
# the optical frame is defined as here http://www.ros.org/reps/rep-0103.html#id21 and follows
# the opencv convention https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
# z forward, x right, y down
opticalToLinkVtk = spartanUtils.transformFromPose(
d['extrinsics']['transform_to_reference_link'])
# body is the body frame of the camera as defined here http://www.ros.org/reps/rep-0103.html#id21
# x forward, y left, z up
bodyToLinkVtk = CameraTransformPublisher.transformOpticalFrameToBodyFrame(
opticalToLinkVtk)
bodyToLinkPoseDict = spartanUtils.poseFromTransform(bodyToLinkVtk)
bodyToLink = rosUtils.ROSTransformMsgFromPose(bodyToLinkPoseDict)
cameraToLinkStamped = geometry_msgs.msg.TransformStamped()
cameraToLinkStamped.transform = bodyToLink
cameraToLinkStamped.child_frame_id = self.cameraName + '_' + cameraType + "_frame"
cameraToLinkStamped.header.frame_id = d['extrinsics'][
'reference_link_name']
data['camera_to_link_transform_stamped'] = cameraToLinkStamped
self.cameraInfo[cameraType] = data
rospy.loginfo('finished parsing data for camera %s',
cameraToLinkStamped.header.frame_id)
def setupConfig(self):
self.config = dict()
self.config['base_frame_id'] = "base"
self.config['end_effector_frame_id'] = "iiwa_link_ee"
self.config[
'pick_up_distance'] = 0.25 # distance to move above the table after grabbing the object
self.config["sleep_time_for_sensor_collect"] = 0.1
self.config['scan'] = dict()
self.config['scan']['pose_list'] = [
'scan_left_close', 'scan_above_table', 'scan_right'
]
self.config['scan']['joint_speed'] = 45
self.config['grasp_speed'] = 20
normal_speed = 30
self.config['speed'] = dict()
self.config['speed']['stow'] = normal_speed
self.config['speed']['pre_grasp'] = normal_speed
self.config['speed']['grasp'] = 10
self.config['home_pself.moveose_name'] = 'above_table_pre_grasp'
self.config['grasp_nominal_direction'] = np.array([1, 0,
0]) # x forwards
self.config['grasp_to_ee'] = dict()
self.config['grasp_to_ee']['translation'] = dict()
# self.config['grasp_to_ee']['translation']['x'] = 9.32362425e-02
self.config['grasp_to_ee']['translation']['x'] = 0.085
self.config['grasp_to_ee']['translation']['y'] = 0
self.config['grasp_to_ee']['translation']['z'] = 0
self.config['grasp_to_ee']['orientation'] = dict()
self.config['grasp_to_ee']['orientation']['w'] = 0.97921432
self.config['grasp_to_ee']['orientation']['x'] = -0.20277454
self.config['grasp_to_ee']['orientation']['y'] = 0.00454233
self.config['grasp_to_ee']['orientation']['z'] = -0.00107904
self.config["object_interaction"] = dict()
self.config["object_interaction"]["speed"] = 10
self.config["object_interaction"]["rotate_speed"] = 30
self.config["object_interaction"]["pickup_distance"] = 0.15
# self.config["object_interaction"]["drop_distance_above_grasp"] = 0.035 # good for shoes
self.config["object_interaction"][
"drop_distance_above_grasp"] = 0.002 # good for mugs
self.config["object_interaction"]["drop_location"] = [
0.65, 0, 0.5
] # z coordinate is overwritten later
self.graspToIiwaLinkEE = spartanUtils.transformFromPose(
self.config['grasp_to_ee'])
self.iiwaLinkEEToGraspFrame = self.graspToIiwaLinkEE.GetLinearInverse()
pos = [-0.15, 0, 0]
quat = [1, 0, 0, 0]
self.preGraspToGraspTransform = transformUtils.transformFromPose(
pos, quat)
def fromConfigFilename(robotSystem, configFilename, channelName):
config = spartanUtils.getDictFromYamlFilename(configFilename)
transformDict = config['depth']['extrinsics'][
'transform_to_reference_link']
cameraToLinkTransform = spartanUtils.transformFromPose(transformDict)
rgbTransformDict = config['rgb']['extrinsics'][
'transform_to_reference_link']
rgbCameraToLinkTransform = spartanUtils.transformFromPose(
rgbTransformDict)
return CameraTransform(
robotSystem,
referenceLinkName=config['depth']['extrinsics']
['reference_link_name'],
cameraToLinkTransform=cameraToLinkTransform,
channelName=channelName,
rgbCameraToLinkTransform=rgbCameraToLinkTransform)
def setupConfig(self):
self.config = dict()
self.config['base_frame_id'] = "base"
self.config['end_effector_frame_id'] = "iiwa_link_ee"
self.config[
'pick_up_distance'] = 0.15 # distance to move above the table after grabbing the object
self.config['scan'] = dict()
self.config['scan']['pose_list'] = [
'scan_left_close', 'scan_above_table', 'scan_right'
]
self.config['scan']['joint_speed'] = 30
self.config['grasp_speed'] = 20
normal_speed = 30
self.config['speed'] = dict()
self.config['speed']['stow'] = normal_speed
self.config['speed']['pre_grasp'] = normal_speed
self.config['speed']['grasp'] = 10
self.config['home_pself.moveose_name'] = 'above_table_pre_grasp'
self.config['grasp_nominal_direction'] = np.array([1, 0,
0]) # x forwards
self.config['grasp_to_ee'] = dict()
self.config['grasp_to_ee']['translation'] = dict()
# self.config['grasp_to_ee']['translation']['x'] = 9.32362425e-02
self.config['grasp_to_ee']['translation']['x'] = 0.085
self.config['grasp_to_ee']['translation']['y'] = 0
self.config['grasp_to_ee']['translation']['z'] = 0
self.config['grasp_to_ee']['orientation'] = dict()
self.config['grasp_to_ee']['orientation']['w'] = 0.97921432
self.config['grasp_to_ee']['orientation']['x'] = -0.20277454
self.config['grasp_to_ee']['orientation']['y'] = 0.00454233
self.config['grasp_to_ee']['orientation']['z'] = -0.00107904
self.graspToIiwaLinkEE = spartanUtils.transformFromPose(
self.config['grasp_to_ee'])
self.iiwaLinkEEToGraspFrame = self.graspToIiwaLinkEE.GetLinearInverse()
pos = [-0.15, 0, 0]
quat = [1, 0, 0, 0]
self.preGraspToGraspTransform = transformUtils.transformFromPose(
pos, quat)