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
Exemplo n.º 3
0
    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
Exemplo n.º 4
0
    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()
Exemplo n.º 5
0
    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)
Exemplo n.º 7
0
    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)
Exemplo n.º 8
0
    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
Exemplo n.º 9
0
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
Exemplo n.º 10
0
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