def handle_compute_ik_divided(self, req): nb_frames = len(req.desired_poses) # convert the desired poses to dict desired_dict = {} for pose in req.desired_poses: desired_dict[pose.header.frame_id] = transformations.pose_to_list(pose) # convert the fixed joint state to dict fixed_joints_dict = {} for i in range(len(req.fixed_joints.name)): fixed_joints_dict[req.fixed_joints.name[i]] = req.fixed_joints.position[i] # create as many threads as nb frames threads = [None] * nb_frames results = {} for i, frame in enumerate(desired_dict.keys()): threads[i] = Thread(target=self.compute_sub_ik, args=(frame, desired_dict, results, req.tolerance)) threads[i].start() # join thread results for i in range(len(threads)): threads[i].join() # convert restult to a joint state js = req.seed js.position = list(js.position) js.name = list(js.name) # replace joint values with optimized ones for key, value in results.iteritems(): for i, name in enumerate(self.joint_by_links[key]): js.position[js.name.index(name)] = value[i] # replace fixed values for key, value in fixed_joints_dict.iteritems(): js.position[js.name.index(key)] = value # return server reply return GetHumanIKResponse(js)
def pathtodict(path): assert isinstance(path, Path) frame_id = path.header.frame_id if frame_id == '' and len(path.poses) > 0: frame_id = path.poses[0].header.frame_id dic = {"frame_id": frame_id, "points": []} for point in path.poses: dic["points"].append({"time": point.header.stamp.to_sec(), "pose": pose_to_list(point)}) return dic
def rollout(self, goal): assert isinstance(goal, PoseStamped) self._dmp.goal = [val for sublist in pose_to_list(goal) for val in sublist] y_track, dy_track, ddy_track = self._dmp.rollout() path = Path() for y in y_track: path.poses.append(list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]], frame_id=self.init_path.header.frame_id)) path.header.stamp = rospy.Time.now() path.header.frame_id = self.init_path.header.frame_id return path
def _path_to_y_des(self, path, nss): y_des = [] for pose_s in path.poses: assert pose_s.header.frame_id == self.init_path.header.frame_id pose = [val for sublist in pose_to_list(pose_s) for val in sublist] # flatten to [x, y, z, x, y, z, w] y_des.append(pose) # Repeat the last point (stable state) n times to avoid brutal cuts due to asymptotic approach for n in range(nss): y_des.append(y_des[-1]) return np.array(y_des).transpose()
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None): solutions = [] for pose_num, eef_pose in enumerate(eef_poses): if eef_pose.header.frame_id.strip('/') != self._world.strip('/'): raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world)) pose = pose_to_list(eef_pose) resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1], [seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)] for joint in self.joint_names()] if len(seeds)>0 else None) if resp is None: rs = None else: rs = RobotState() rs.is_diff = False rs.joint_state.name = self.joint_names() rs.joint_state.position = resp solutions.append(rs) return solutions
def handle_compute_ik(self, req): joint_names_set = set() # get the number of joints according to the required group for g in req.group_names: joint_names_set.update(self.model.get_joint_names(g)) self.joint_names = list(joint_names_set) # convert the desired poses to dict desired_dict = {} for pose in req.desired_poses: desired_dict[pose.header.frame_id] = transformations.pose_to_list(pose) # convert the fixed joint state to dict fixed_joints_dict = {} for i in range(len(req.fixed_joints.name)): fixed_joints_dict[req.fixed_joints.name[i]] = req.fixed_joints.position[i] # get initial state init_state = req.seed # get only joints to optimize init_joints = [] for name in self.joint_names: init_joints.append(init_state.position[init_state.name.index(name)]) # get the joints limits for the optimization joint_limits = self.model.get_joint_limits(self.joint_names) # optimize to find the corresponding IK res = minimize(self.cost_function, init_joints, # jac=self.jacobian_cost_function, args=(desired_dict, fixed_joints_dict, ), method='L-BFGS-B', bounds=joint_limits) opt_joints = list(res.x) # convert it to a joint state js = self.model.get_current_state() js.position = list(js.position) js.name = list(js.name) # replace joint values with optimized ones for i in range(len(self.joint_names)): name = self.joint_names[i] js.position[js.name.index(name)] = opt_joints[i] # replace fixed values for key, value in fixed_joints_dict.iteritems(): js.position[js.name.index(key)] = value # return server reply return GetHumanIKResponse(js)
def add_demonstration(self, demonstration, js_demo): """ Add a new task-space demonstration and update the model :param demonstration: Path object :param js_demo: Joint space demo for duration TODO replace Path by something else including duration? :return: """ if not isinstance(demonstration, Path): raise TypeError("ros.TaskProMP.add_demonstration only accepts Path, got {}".format(type(demonstration))) if isinstance(js_demo, RobotTrajectory): js_demo = js_demo.joint_trajectory elif not isinstance(js_demo, JointTrajectory): raise TypeError("ros.ProMP.add_demo only accepts RT or JT, got {}".format(type(js_demo))) self._durations.append( js_demo.points[-1].time_from_start.to_sec() - js_demo.points[0].time_from_start.to_sec()) self.joint_names = js_demo.joint_names demo_array = [list_to_raw_list(pose_to_list(pose)) for pose in demonstration.poses] self.promp.add_demonstration(demo_array)
def _get_ik_pykdl(self, eef_poses, seeds=(), params=None): solutions = [] for pose_num, eef_pose in enumerate(eef_poses): if eef_pose.header.frame_id.strip('/') != self._world.strip('/'): raise NotImplementedError( "_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}" .format(self._world)) pose = pose_to_list(eef_pose) resp = self._kinematics_pykdl.inverse_kinematics( pose[0], pose[1], [ seeds[pose_num].joint_state.position[ seeds[pose_num].joint_state.name.index(joint)] for joint in self.joint_names() ] if len(seeds) > 0 else None) if resp is None: rs = None else: rs = RobotState() rs.is_diff = False rs.joint_state.name = self.joint_names() rs.joint_state.position = resp solutions.append(rs) return solutions
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