def get_place_pose(self, location_T_object, object, location, viapoint=None): # Computes the world_T_gripper transform, taking into account the viapoint if it exists location_T_object = deepcopy(location_T_object) try: object_T_gripper = self.tfl.lookupTransform( object, self.gripper, rospy.Time(0)) world_T_location = self.tfl.lookupTransform( self.world, location, rospy.Time(0)) except KeyError: rospy.logerr("No declared pose to place {} at {}".format( object, location)) return None if viapoint is not None: location_T_object[0] = list( array(location_T_object[0]) + array(self.poses[location]['place']['via'])) location_T_gripper = transformations.multiply_transform( location_T_object, object_T_gripper) world_T_gripper = transformations.multiply_transform( world_T_location, location_T_gripper) return world_T_gripper
def update_frame(target, prefix): base = self.human_prefix + '/base' # loop through all the prefixes for b_pref in prefix: for t_pref in prefix: # check visibility of frame if self.tfl.canTransform(b_pref + '/' + base, t_pref + '/' + target, rospy.Time(0)): time = self.tfl.getLatestCommonTime( b_pref + '/' + base, t_pref + '/' + target) if rospy.Time.now() - time < rospy.Duration(0.5): frame = self.tfl.lookupTransform( b_pref + '/' + base, t_pref + '/' + target, time) # multiply each transformation by the calibration matrix if self.calibrated: dot_prod = transformations.multiply_transform( self.calibration[b_pref][base + '/inv'], frame) dot_prod = transformations.multiply_transform( dot_prod, self.calibration[t_pref][target]) self.skel_data[target] = [ self.sensors_ref[t_pref], dot_prod ] else: self.skel_data[target] = [ self.sensors_ref[t_pref], frame ] return True if debug: rospy.logwarn(target + ' not visible') return False
def update_frame(target, prefix): base = self.human_prefix + '/base' # loop through all the prefixes for b_pref in prefix: for t_pref in prefix: # check visibility of frame if self.tfl.canTransform(b_pref + '/' + base, t_pref + '/' + target, rospy.Time(0)): time = self.tfl.getLatestCommonTime(b_pref + '/' + base, t_pref + '/' + target) if rospy.Time.now() - time < rospy.Duration(0.5): frame = self.tfl.lookupTransform(b_pref + '/' + base, t_pref + '/' + target, time) # multiply each transformation by the calibration matrix if self.calibrated: dot_prod = transformations.multiply_transform(self.calibration[b_pref][base + '/inv'], frame) dot_prod = transformations.multiply_transform(dot_prod, self.calibration[t_pref][target]) self.skel_data[target] = [self.sensors_ref[t_pref], dot_prod] else: self.skel_data[target] = [self.sensors_ref[t_pref], frame] return True if debug: rospy.logwarn(target + ' not visible') return False
def _evaluate_calibration(self, calibrations): def quaternion_cost(norm_coeff): C = 0 for transform in list_calibr: # norm of a quaternion is always 1 C += norm_coeff * abs(np.linalg.norm(transform[1]) - 1) return C def distance_cost(pose1, pose2, rot_coeff=2): pos_cost = 0 # calculate position ditance pos_cost = np.linalg.norm(np.array(pose1[0]) - np.array(pose2[0])) # distance between two quaternions rot_cost = 1 - np.inner(pose1[1], pose2[1])**2 return pos_cost + rot_coeff * rot_cost def base_cost(base_pose, coeff=2): q = base_pose[1] # calculate z axis from quaternion z = [ 2 * (q[0] * q[2] + q[1] * q[3]), 2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 * q[0] * q[0] - 2 * q[1] * q[1] ] # check that the z base axis is normal to the ground cost = coeff * abs(np.dot(z, self.ground_axis) - 1) return cost # first extract the transformations list_calibr = self.extract_transforms(calibrations) # collect the cost based on the quaternions cost = quaternion_cost(0.5) # set the base transform base_transform = list_calibr[0] inv_base = transformations.inverse_transform(base_transform) # get pose of the base base_pose = transformations.multiply_transform( self.recorded_poses[self.human.prefix + '/base'], inv_base) # calculate cost of the base cost += (10 * base_cost(base_pose)) # loop trough all the transforms for i in range(1, len(list_calibr)): key = self.keys[i] # get the fk fk = self.fk[key] # compute the corresponding transformation from recorded data pose = transformations.multiply_transform(base_transform, self.recorded_poses[key]) pose = transformations.multiply_transform( pose, transformations.inverse_transform(list_calibr[i])) pose[1] /= np.linalg.norm(pose[1]) # compute the cost based on the distance cost += distance_cost(fk, pose) print cost return cost
def _evaluate_calibration(self, calibrations): def quaternion_cost(norm_coeff): C = 0 for transform in list_calibr: # norm of a quaternion is always 1 C += norm_coeff * abs(np.linalg.norm(transform[1]) - 1) return C def distance_cost(pose1, pose2, rot_coeff=2): pos_cost = 0 # calculate position ditance pos_cost = np.linalg.norm(np.array(pose1[0]) - np.array(pose2[0])) # distance between two quaternions rot_cost = 1 - np.inner(pose1[1], pose2[1])**2 return pos_cost + rot_coeff * rot_cost def base_cost(base_pose, coeff=2): q = base_pose[1] # calculate z axis from quaternion z = [2 * (q[0] * q[2] + q[1] * q[3]), 2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 * q[0] * q[0] - 2 * q[1] * q[1]] # check that the z base axis is normal to the ground cost = coeff * abs(np.dot(z, self.ground_axis) - 1) return cost # first extract the transformations list_calibr = self.extract_transforms(calibrations) # collect the cost based on the quaternions cost = quaternion_cost(0.5) # set the base transform base_transform = list_calibr[0] inv_base = transformations.inverse_transform(base_transform) # get pose of the base base_pose = transformations.multiply_transform(self.recorded_poses[self.human.prefix + '/base'], inv_base) # calculate cost of the base cost += (10 * base_cost(base_pose)) # loop trough all the transforms for i in range(1, len(list_calibr)): key = self.keys[i] # get the fk fk = self.fk[key] # compute the corresponding transformation from recorded data pose = transformations.multiply_transform(base_transform, self.recorded_poses[key]) pose = transformations.multiply_transform(pose, transformations.inverse_transform(list_calibr[i])) pose[1] /= np.linalg.norm(pose[1]) # compute the cost based on the distance cost += distance_cost(fk, pose) print cost return cost
def update_base_frame(prefix): for pref in prefix: ref = self.sensors_ref[pref] + '_frame' if self.tfl.canTransform( ref, pref + '/' + self.human_prefix + '/base', rospy.Time(0)): time = self.tfl.getLatestCommonTime( ref, pref + '/' + self.human_prefix + '/base') if rospy.Time.now() - time < rospy.Duration(0.5): frame = self.tfl.lookupTransform( ref, pref + '/' + self.human_prefix + '/base', time) # multiply with the calibration if self.calibrated: dot_prod = transformations.multiply_transform( frame, self.calibration[pref][self.human_prefix + '/base']) self.skel_data[self.human_prefix + '/base'] = [ self.sensors_ref[pref], dot_prod ] else: self.skel_data[self.human_prefix + '/base'] = [ self.sensors_ref[pref], frame ] return True if debug: rospy.logwarn(self.human_prefix + '/base not visible') return False
def pred_positioned(self, master, slave, atp): """ Checks if any constraint between master and slave exists at attach point atp and returns True if the constraint is within the tolerance :param master: :param slave: :param atp: (int) :return: True if predicate POSITIONED(master, slave, atp) is True """ if master + slave + str(atp) in self.attached: return True try: # WARNING: Do not ask the relative tf directly, it is outdated! tf_slave = self.tfl.lookupTransform(self.world, slave, rospy.Time(0)) tf_master = self.tfl.lookupTransform(self.world, master, rospy.Time(0)) except Exception as e: pass else: relative = transformations.multiply_transform( transformations.inverse_transform(tf_master), tf_slave) constraint = self.poses[master]['constraints'][atp][slave] cart_dist = transformations.distance(constraint, relative) quat_dist = transformations.distance_quat(constraint, relative) return cart_dist < self.config['positioned'][ 'position_tolerance'] and quat_dist < self.config[ 'positioned']['orientation_tolerance'] return False
def compute_sub_ik(self, group, desired_dict, result, tol=0.001): # get the desired pose in the correct base frame index = self.links.index(group) base = self.bases[index] tr = desired_dict[group] if base != self.prefix + '/base': if base in desired_dict.keys(): tr_base = desired_dict[base] inv_base = transformations.inverse_transform(tr_base) desired_pose = transformations.multiply_transform(inv_base, tr) else: return 1 else: desired_pose = tr # transform it back to PoseStamped desired_pose = transformations.list_to_pose(desired_pose) try: # call the srv res = self.div_ik_srv[group](desired_poses=[desired_pose], tolerance=tol) joints = res.joint_state.position except: return 1 with self.lock: result[group] = joints
def compute_sub_ik(self, group, desired_dict, result, seed, tol=0.1): # get the desired pose in the correct base frame base = self.links[group] tr = desired_dict[group] if desired_dict: base_found = False if base != self.prefix + '/base': if base in desired_dict.keys(): base_found = True tr_base = desired_dict[base] else: # look for the base in already calculated results timeout = 0.25 start = time() while not base_found and not rospy.is_shutdown() and ( time() - start < timeout): with self.lock: for key, value in result.iteritems(): if base in self.model.get_links_chain( self.links[key], key): base_found = True js = JointState() js.name = value['joint_names'] js.position = value['joint_values'] # call the forward kinematic fk = self.model.forward_kinematic( js, links=base) tr_base = fk[base] if base_found: inv_base = transformations.inverse_transform(tr_base) desired_pose = transformations.multiply_transform( inv_base, tr) else: return 1 else: desired_pose = tr else: return 1 # transform it back to PoseStamped desired_pose = transformations.list_to_pose(desired_pose) desired_pose.header.frame_id = base # try: # call the srv res = self.div_ik_srv[group](desired_poses=[desired_pose], tolerance=tol, seed=seed) joint_state = res.joint_state # except Exception, e: # print e # return 1 with self.lock: result[group] = { 'joint_names': joint_state.name, 'joint_values': joint_state.position }
def pred_position(self, master, slave, atp): try: # WARNING: Do not ask the relative tf directly, it is outdated! tf_slave = self.tfl.lookupTransform(self.world, slave, rospy.Time(0)) tf_master = self.tfl.lookupTransform(self.world, master, rospy.Time(0)) except Exception: pass else: relative = transformations.multiply_transform( transformations.inverse_transform(tf_master), tf_slave) constraint = self.poses[master]['constraints'][atp][slave] cart_dist = transformations.distance(constraint, relative) quat_dist = transformations.distance_quat(constraint, relative) return (cart_dist < self.config['start_position']['position_tolerance'] and quat_dist < self.config['start_position']['orientation_tolerance']) and\ not (cart_dist < self.config['positioned']['position_tolerance'] and quat_dist < self.config['positioned']['orientation_tolerance'])
def pred_attached(self, master, slave, atp): if master + slave + str(atp) in self.attached: return True elif self.pred_positioned(master, slave, atp): try: # WARNING: Do not ask the relative tf directly, it is outdated! screwdriver = self.tfl.lookupTransform(self.world, self.screwdriver, rospy.Time(0)) tf_master = self.tfl.lookupTransform(self.world, master, rospy.Time(0)) except: pass else: if self.screwdriver in self.poses[master]['constraints'][ atp]: # For objects that need to be screwed relative = transformations.multiply_transform( transformations.inverse_transform(tf_master), screwdriver) cart_dist = transformations.distance( relative, self.poses[master]['constraints'][atp][ self.screwdriver]) if cart_dist < self.config['attached'][ 'tool_position_tolerance']: try: if rospy.Time.now() - self.attaching_stamps[ master][slave] > rospy.Duration( self.config['attached'] ['screwdriver_attaching_time']): self.attaching_started.append(master + slave + str(atp)) except KeyError: if not self.attaching_stamps.has_key(master): self.attaching_stamps[master] = {} self.attaching_stamps[master][ slave] = rospy.Time.now() elif master + slave + str(atp) in self.attaching_started: self.attached.append(master + slave + str(atp)) else: # For objects that only need to be inserted # self.screwed.append(master+slave+str(atp)) self.attached.append(master + slave + str(atp)) return False
def update_base_frame(prefix): for pref in prefix: ref = self.sensors_ref[pref] + '_frame' if self.tfl.canTransform(ref, pref + '/' + self.human_prefix + '/base', rospy.Time(0)): time = self.tfl.getLatestCommonTime(ref, pref + '/' + self.human_prefix + '/base') if rospy.Time.now() - time < rospy.Duration(0.5): frame = self.tfl.lookupTransform(ref, pref + '/' + self.human_prefix + '/base', time) # multiply with the calibration if self.calibrated: dot_prod = transformations.multiply_transform(frame, self.calibration[pref][self.human_prefix + '/base']) self.skel_data[self.human_prefix + '/base'] = [self.sensors_ref[pref], dot_prod] else: self.skel_data[self.human_prefix + '/base'] = [self.sensors_ref[pref], frame] return True if debug: rospy.logwarn(self.human_prefix + '/base not visible') return False
def pred_screw(self, master, slave, atp, state): if Predicate(type='positioned', parameters=[master, slave, str(atp)]) in state.predicates: try: # WARNING: Do not ask the relative tf directly, it is outdated! screwdriver = self.tfl.lookupTransform(self.world, self.screwdriver, rospy.Time(0)) tf_master = self.tfl.lookupTransform(self.world, master, rospy.Time(0)) except: pass else: relative = transformations.multiply_transform( transformations.inverse_transform(tf_master), screwdriver) cart_dist = transformations.distance( relative, self.poses[master]['constraints'][atp][self.screwdriver]) #quat_dist = transformations.distance_quat(relative, self.poses[master]['constraints'][atp][self.screwdriver]) # Do not measure orientation, since the screwdriver has to spin to screw return cart_dist < self.config['attached'][ 'tool_position_tolerance'] return False
class HumanModel(object): def __init__(self, description='human_description', prefix='human', control=False): rospack = RosPack() self.path = rospack.get_path('human_moveit_config') self.description = description self.robot_commander = moveit_commander.RobotCommander(description) if control: self.joint_publisher = rospy.Publisher('/human/set_joint_values', JointState, queue_size=1) self.groups = {} self.groups['head'] = moveit_commander.MoveGroupCommander( 'Head', description) self.groups['right_arm'] = moveit_commander.MoveGroupCommander( 'RightArm', description) self.groups['left_arm'] = moveit_commander.MoveGroupCommander( 'LeftArm', description) self.groups['right_leg'] = moveit_commander.MoveGroupCommander( 'RightLeg', description) self.groups['left_leg'] = moveit_commander.MoveGroupCommander( 'LeftLeg', description) self.groups['upper_body'] = moveit_commander.MoveGroupCommander( 'UpperBody', description) self.groups['lower_body'] = moveit_commander.MoveGroupCommander( 'LowerBody', description) self.groups['whole_body'] = moveit_commander.MoveGroupCommander( 'WholeBody', description) # initialize end-effectors dict self.end_effectors = {} # fill both dict for key, value in self.groups.iteritems(): self.end_effectors[key] = value.get_end_effector_link() # add the list of end-effectors for bodies self.end_effectors['upper_body'] = [ self.end_effectors['head'], self.end_effectors['right_arm'], self.end_effectors['left_arm'] ] self.end_effectors['lower_body'] = [ self.end_effectors['right_leg'], self.end_effectors['left_leg'] ] self.end_effectors['whole_body'] = self.end_effectors[ 'upper_body'] + self.end_effectors['lower_body'] # initialize common links per group self.group_links = {} self.group_links['head'] = [ prefix + '/shoulder_center', prefix + '/head' ] # fill the disct of active joints by links self.joint_by_links = {} self.joint_by_links[prefix + '/shoulder_center'] = [ 'spine_0', 'spine_1', 'spine_2' ] self.joint_by_links[prefix + '/head'] = ['neck_0', 'neck_1', 'neck_2'] sides = ['right', 'left'] for s in sides: self.group_links[s + '_arm'] = [ prefix + '/' + s + '_shoulder', prefix + '/' + s + '_elbow', prefix + '/' + s + '_hand' ] self.group_links[s + '_leg'] = [ prefix + '/' + s + '_hip', prefix + '/' + s + '_knee', prefix + '/' + s + '_foot' ] # arm links self.joint_by_links[prefix + '/' + s + '_shoulder'] = [ s + '_shoulder_0', s + '_shoulder_1', s + '_shoulder_2' ] self.joint_by_links[prefix + '/' + s + '_elbow'] = [s + '_elbow_0', s + '_elbow_1'] self.joint_by_links[prefix + '/' + s + '_hand'] = [s + '_wrist_0', s + '_wrist_1'] # leg links self.joint_by_links[prefix + '/' + s + '_hip'] = [ s + '_hip_0', s + '_hip_1', s + '_hip_2' ] self.joint_by_links[prefix + '/' + s + '_knee'] = [s + '_knee'] self.joint_by_links[prefix + '/' + s + '_foot'] = [s + '_ankle_0', s + '_ankle_1'] self.prefix = prefix rospy.wait_for_service('compute_fk') self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK) self.current_state = self.get_initial_state() def init_nearest_neighbour_trees(self): def load_database(link): database = np.load( join(self.path, 'database8', 'database_' + link + '.npz')) data_dict = {} data_dict['data'] = database if self.with_orient: data_dict['tree'] = cKDTree(database['data'][:, -7:]) else: data_dict['tree'] = cKDTree(database['data'][:, -7:-4]) return data_dict self.trees = {} links = ['head', 'shoulder_center'] for s in ['right', 'left']: links += [s + '_elbow', s + '_hand'] for l in links: self.trees[l] = load_database(l) def get_group_of_link(self, link): for key, value in self.group_links.iteritems(): if link in value: group = key break return group def extract_group_joints(self, group_name, joint_state): active = self.groups[group_name].get_active_joints() res = [] for joint in active: index = joint_state.name.index(joint) res.append(joint_state.position[index]) return res def forward_kinematic(self, joint_state, base='base', links=None): def compute_fk_client(): try: header = Header() header.stamp = rospy.Time.now() header.frame_id = self.prefix + '/base' rs = RobotState() rs.joint_state = joint_state res = self.compute_fk(header, links, rs) return res.pose_stamped except rospy.ServiceException, e: print "Service call failed: %s" % e # in case of troubles return 0 pose stamped return [] if links is None: links = self.end_effectors['whole_body'] if type(links) is not list: if links == "all": links = self.get_link_names('whole_body') else: links = [links] # check that the base is in links if base != 'base' and base not in links: links.append(base) pose_stamped_list = compute_fk_client() if not pose_stamped_list: return {} # transform it in a dict of poses pose_dict = {} if base != 'base': tr_base = transformations.pose_to_list( pose_stamped_list[links.index(base)].pose) inv_base = transformations.inverse_transform(tr_base) for i in range(len(links)): if links[i] != base: tr = transformations.pose_to_list( pose_stamped_list[i].pose) pose_dict[links[i]] = transformations.multiply_transform( inv_base, tr) else: for i in range(len(links)): pose_dict[links[i]] = transformations.pose_to_list( pose_stamped_list[i].pose) return pose_dict
class HumanModel(object): def __init__(self, description='human_description', prefix='human', control=False): self.description = description self.robot_commander = RobotCommander(description) if control: self.joint_publisher = rospy.Publisher('/human/set_joint_values', JointState, queue_size=1) self.groups = {} self.groups['head'] = MoveGroupCommander('Head', description) self.groups['right_arm'] = MoveGroupCommander('RightArm', description) self.groups['left_arm'] = MoveGroupCommander('LeftArm', description) self.groups['right_leg'] = MoveGroupCommander('RightLeg', description) self.groups['left_leg'] = MoveGroupCommander('LeftLeg', description) self.groups['upper_body'] = MoveGroupCommander('UpperBody', description) self.groups['lower_body'] = MoveGroupCommander('LowerBody', description) self.groups['whole_body'] = MoveGroupCommander('WholeBody', description) # initialize end-effectors dict self.end_effectors = {} # fill both dict for key, value in self.groups.iteritems(): self.end_effectors[key] = value.get_end_effector_link() # add the list of end-effectors for bodies self.end_effectors['upper_body'] = [ self.end_effectors['head'], self.end_effectors['right_arm'], self.end_effectors['left_arm'] ] self.end_effectors['lower_body'] = [ self.end_effectors['right_leg'], self.end_effectors['left_leg'] ] self.end_effectors['whole_body'] = self.end_effectors[ 'upper_body'] + self.end_effectors['lower_body'] self.prefix = prefix self.urdf_reader = URDFReader() rospy.wait_for_service('compute_fk') self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK) self.current_state = self.get_initial_state() def find_common_root(self, link1, link2): return self.urdf_reader.find_common_root(link1, link2) def get_links_chain(self, from_link, to_link): return self.urdf_reader.get_links_chain(from_link, to_link) def get_joints_chain(self, from_link, to_link): return self.urdf_reader.get_joints_chain(from_link, to_link) def extract_group_joints(self, group_name, joint_state): active = self.groups[group_name].get_active_joints() res = [] for joint in active: index = joint_state.name.index(joint) res.append(joint_state.position[index]) return res def forward_kinematic(self, joint_state, base='base', links=None): def compute_fk_client(): try: header = Header() header.stamp = rospy.Time.now() header.frame_id = self.prefix + '/base' rs = RobotState() rs.joint_state = joint_state res = self.compute_fk(header, links, rs) return res.pose_stamped except rospy.ServiceException, e: print "Service call failed: %s" % e # in case of troubles return 0 pose stamped return [] if links is None: links = self.end_effectors['whole_body'] if type(links) is not list: if links == "all": links = self.get_link_names('whole_body') else: links = [links] # check that the base is in links if base != 'base' and base not in links: links.append(base) pose_stamped_list = compute_fk_client() if not pose_stamped_list: return {} # transform it in a dict of poses pose_dict = {} if base != 'base': tr_base = transformations.pose_to_list( pose_stamped_list[links.index(base)].pose) inv_base = transformations.inverse_transform(tr_base) for i in range(len(links)): if links[i] != base: tr = transformations.pose_to_list( pose_stamped_list[i].pose) pose_dict[links[i]] = transformations.multiply_transform( inv_base, tr) else: for i in range(len(links)): pose_dict[links[i]] = transformations.pose_to_list( pose_stamped_list[i].pose) return pose_dict
def run(self, parameters=None): rospy.loginfo("[ActionServer] Executing bring{}".format( str(parameters))) object = parameters[0] wrist = '/human/wrist' if not self.commander.gripping(): rospy.logerr('Object {} is no longer gripped'.format(object)) return False rospy.loginfo("Bringing {} to the human".format(object)) while not self._should_interrupt(): try: distance_object_location = transformations.distance( self.tfl.lookupTransform(wrist, object, rospy.Time(0)), self.poses[object]['bring'][wrist]) object_T_gripper = self.tfl.lookupTransform( object, self.gripper_name, rospy.Time(0)) world_T_location = self.tfl.lookupTransform( self.world, wrist, rospy.Time(0)) except KeyError: rospy.logerr("No declared pose to bring {}".format(object)) return False except: rospy.logwarn("{} or {} not found".format(object, wrist)) rospy.sleep(self.action_params['sleep_step']) continue rospy.loginfo("{} at {}m from {}, threshold {}m".format( object, distance_object_location, wrist, self.action_params['bring']['sphere_radius'])) if distance_object_location > self.action_params['bring'][ 'sphere_radius']: location_T_gripper = transformations.multiply_transform( self.poses[object]['bring'][wrist], object_T_gripper) world_T_gripper = transformations.multiply_transform( world_T_location, location_T_gripper) try: success = self.commander.move_to_controlled( world_T_gripper, rpy=[1, 1, 0], pause_test=self.pause_test, stop_test=self.stop_test) except ValueError: rospy.logwarn( "Human wrist found but not reachable, please move it a little bit..." .format(wrist)) rospy.sleep(self.action_params['sleep_step']) continue else: if not success: return False else: break rospy.loginfo("[ActionServer] Executed bring{} with {}".format( str(parameters), "failure" if self._should_interrupt() else "success")) return True