Ejemplo n.º 1
0
class GiskardTestWrapper(object):
    def __init__(self, config_file):
        with open(get_ros_pkg_path(u'giskardpy') + u'/config/' +
                  config_file) as f:
            config = yaml.load(f)
        rospy.set_param('~', config)
        rospy.set_param('~path_to_data_folder', u'tmp_data/')
        rospy.set_param('~enable_gui', False)
        rospy.set_param('~plugins/PlotTrajectory/enabled', True)

        self.sub_result = rospy.Subscriber('~command/result',
                                           MoveActionResult,
                                           self.cb,
                                           queue_size=100)
        self.cancel_goal = rospy.Publisher('~command/cancel',
                                           GoalID,
                                           queue_size=100)

        self.tree = grow_tree()
        self.loop_once()
        # rospy.sleep(1)
        self.wrapper = GiskardWrapper(node_name=u'tests')
        self.results = Queue(100)
        self.default_root = self.get_robot().get_root()
        self.map = u'map'
        self.simple_base_pose_pub = rospy.Publisher('/move_base_simple/goal',
                                                    PoseStamped,
                                                    queue_size=10)
        self.set_base = rospy.ServiceProxy('/base_simulator/set_joint_states',
                                           SetJointState)
        self.tick_rate = 10

        def create_publisher(topic):
            p = rospy.Publisher(topic, JointState, queue_size=10)
            rospy.sleep(.2)
            return p

        self.joint_state_publisher = KeyDefaultDict(create_publisher)
        # rospy.sleep(1)

    def wait_for_synced(self):
        sleeper = rospy.Rate(self.tick_rate)
        self.loop_once()
        # while self.tree.tip().name != u'has goal':
        #     self.loop_once()
        #     sleeper.sleep()
        # self.loop_once()
        # while self.tree.tip().name != u'has goal':
        #     self.loop_once()
        #     sleeper.sleep()

    def get_robot(self):
        """
        :rtype: Robot
        """
        return self.get_god_map().get_data(robot)

    def get_god_map(self):
        """
        :rtype: giskardpy.god_map.GodMap
        """
        return Blackboard().god_map

    def cb(self, msg):
        """
        :type msg: MoveActionResult
        """
        self.results.put(msg.result)

    def loop_once(self):
        self.tree.tick()

    def get_controlled_joint_names(self):
        """
        :rtype: dict
        """
        return self.get_robot().controlled_joints

    def get_controllable_links(self):
        return self.get_robot().get_controlled_links()

    def get_current_joint_state(self):
        """
        :rtype: JointState
        """
        return rospy.wait_for_message('joint_states', JointState)

    def tear_down(self):
        rospy.sleep(1)
        logging.loginfo(u'stopping plugins')

    def set_object_joint_state(self, object_name, joint_state):
        self.wrapper.set_object_joint_state(object_name, joint_state)
        rospy.sleep(0.5)
        self.wait_for_synced()
        current_js = self.get_world().get_object(object_name).joint_state
        for joint_name, state in joint_state.items():
            np.testing.assert_almost_equal(current_js[joint_name].position,
                                           state, 2)

    def set_kitchen_js(self, joint_state, object_name=u'kitchen'):
        self.set_object_joint_state(object_name, joint_state)

    #
    # JOINT GOAL STUFF #################################################################################################
    #

    def compare_joint_state(self, current_js, goal_js, decimal=2):
        """
        :type current_js: dict
        :type goal_js: dict
        :type decimal: int
        """
        joint_names = set(current_js.keys()).intersection(set(goal_js.keys()))
        for joint_name in joint_names:
            goal = goal_js[joint_name]
            current = current_js[joint_name]
            if self.get_robot().is_joint_continuous(joint_name):
                np.testing.assert_almost_equal(shortest_angular_distance(
                    goal, current),
                                               0,
                                               decimal=decimal)
            else:
                np.testing.assert_almost_equal(
                    current,
                    goal,
                    decimal,
                    err_msg='{} at {} insteand of {}'.format(
                        joint_name, current, goal))

    def set_joint_goal(self, js, weight=None):
        """
        :rtype js: dict
        """
        self.wrapper.set_joint_goal(js, weight=weight)

    def check_joint_state(self, expected, decimal=2):
        current_joint_state = to_joint_state_position_dict(
            self.get_current_joint_state())
        self.compare_joint_state(current_joint_state,
                                 expected,
                                 decimal=decimal)

    def send_and_check_joint_goal(self,
                                  goal,
                                  weight=None,
                                  decimal=2,
                                  expected_error_codes=None):
        """
        :type goal: dict
        """
        self.set_joint_goal(goal, weight=weight)
        self.send_and_check_goal(expected_error_codes=expected_error_codes)
        if expected_error_codes == [MoveResult.SUCCESS]:
            self.check_joint_state(goal, decimal=decimal)

    #
    # CART GOAL STUFF ##################################################################################################
    #
    def teleport_base(self, goal_pose):
        goal_pose = transform_pose(self.default_root, goal_pose)
        js = {
            u'odom_x_joint':
            goal_pose.pose.position.x,
            u'odom_y_joint':
            goal_pose.pose.position.y,
            u'odom_z_joint':
            rotation_from_matrix(
                quaternion_matrix([
                    goal_pose.pose.orientation.x, goal_pose.pose.orientation.y,
                    goal_pose.pose.orientation.z, goal_pose.pose.orientation.w
                ]))[0]
        }
        goal = SetJointStateRequest()
        goal.state = position_dict_to_joint_states(js)
        self.set_base.call(goal)
        self.loop_once()
        rospy.sleep(0.5)
        self.loop_once()

    def keep_position(self, tip, root=None):
        if root is None:
            root = self.default_root
        goal = PoseStamped()
        goal.header.frame_id = tip
        goal.pose.orientation.w = 1
        self.set_cart_goal(goal, tip, root)

    def keep_orientation(self, tip, root=None):
        goal = PoseStamped()
        goal.header.frame_id = tip
        goal.pose.orientation.w = 1
        self.set_rotation_goal(goal, tip, root)

    def align_planes(self,
                     tip_link,
                     tip_normal,
                     root_link=None,
                     root_normal=None,
                     weight=None):
        self.wrapper.align_planes(tip_link, tip_normal, root_link, root_normal,
                                  weight)

    def set_rotation_goal(self,
                          goal_pose,
                          tip_link,
                          root_link=None,
                          max_velocity=None):
        if not root_link:
            root_link = self.default_root
        self.wrapper.set_rotation_goal(root_link,
                                       tip_link,
                                       goal_pose,
                                       max_velocity=max_velocity)

    def set_translation_goal(self,
                             goal_pose,
                             tip_link,
                             root_link=None,
                             max_velocity=None):
        if not root_link:
            root_link = self.default_root
        self.wrapper.set_translation_goal(root_link,
                                          tip_link,
                                          goal_pose,
                                          max_velocity=max_velocity)

    def set_cart_goal(self,
                      goal_pose,
                      tip_link,
                      root_link=None,
                      weight=None,
                      linear_velocity=None,
                      angular_velocity=None):
        if not root_link:
            root_link = self.default_root
        if weight is not None:
            self.wrapper.set_cart_goal(root_link,
                                       tip_link,
                                       goal_pose,
                                       weight=weight,
                                       max_linear_velocity=linear_velocity,
                                       max_angular_velocity=angular_velocity)
        else:
            self.wrapper.set_cart_goal(root_link,
                                       tip_link,
                                       goal_pose,
                                       max_linear_velocity=linear_velocity,
                                       max_angular_velocity=angular_velocity)

    def set_and_check_cart_goal(self,
                                goal_pose,
                                tip_link,
                                root_link=None,
                                weight=None,
                                linear_velocity=None,
                                angular_velocity=None,
                                expected_error_codes=None):
        goal_pose_in_map = transform_pose(u'map', goal_pose)
        self.set_cart_goal(goal_pose,
                           tip_link,
                           root_link,
                           weight,
                           linear_velocity=linear_velocity,
                           angular_velocity=angular_velocity)
        self.loop_once()
        self.send_and_check_goal(expected_error_codes)
        self.loop_once()
        if expected_error_codes is None:
            self.check_cart_goal(tip_link, goal_pose_in_map)

    def check_cart_goal(self, tip_link, goal_pose):
        goal_in_base = transform_pose(u'map', goal_pose)
        current_pose = lookup_pose(u'map', tip_link)
        np.testing.assert_array_almost_equal(
            msg_to_list(goal_in_base.pose.position),
            msg_to_list(current_pose.pose.position),
            decimal=2)

        try:
            np.testing.assert_array_almost_equal(
                msg_to_list(goal_in_base.pose.orientation),
                msg_to_list(current_pose.pose.orientation),
                decimal=2)
        except AssertionError:
            np.testing.assert_array_almost_equal(
                msg_to_list(goal_in_base.pose.orientation),
                -np.array(msg_to_list(current_pose.pose.orientation)),
                decimal=2)

    #
    # GENERAL GOAL STUFF ###############################################################################################
    #

    def interrupt(self):
        self.cancel_goal.publish(GoalID())

    def check_reachability(self, expected_error_codes=None):
        self.send_and_check_goal(
            expected_error_codes=expected_error_codes,
            goal_type=MoveGoal.PLAN_AND_CHECK_REACHABILITY)

    def get_as(self):
        return Blackboard().get(u'~command')

    def send_goal(self,
                  goal=None,
                  goal_type=MoveGoal.PLAN_AND_EXECUTE,
                  wait=True):
        """
        :rtype: MoveResult
        """
        if goal is None:
            goal = MoveActionGoal()
            goal.goal = self.wrapper._get_goal()
            goal.goal.type = goal_type
        i = 0
        self.loop_once()
        t1 = Thread(
            target=self.get_as()._as.action_server.internal_goal_callback,
            args=(goal, ))
        self.loop_once()
        t1.start()
        sleeper = rospy.Rate(self.tick_rate)
        while self.results.empty():
            self.loop_once()
            sleeper.sleep()
            i += 1
        t1.join()
        self.loop_once()
        result = self.results.get()
        return result

    def send_goal_and_dont_wait(self,
                                goal=None,
                                goal_type=MoveGoal.PLAN_AND_EXECUTE,
                                stop_after=20):
        if goal is None:
            goal = MoveActionGoal()
            goal.goal = self.wrapper._get_goal()
            goal.goal.type = goal_type
        i = 0
        self.loop_once()
        t1 = Thread(
            target=self.get_as()._as.action_server.internal_goal_callback,
            args=(goal, ))
        self.loop_once()
        t1.start()
        sleeper = rospy.Rate(self.tick_rate)
        while self.results.empty():
            self.loop_once()
            sleeper.sleep()
            i += 1
            if i > stop_after:
                self.interrupt()
        t1.join()
        self.loop_once()
        result = self.results.get()
        return result

    def send_and_check_goal(self,
                            expected_error_codes=None,
                            goal_type=MoveGoal.PLAN_AND_EXECUTE,
                            goal=None):
        r = self.send_goal(goal=goal, goal_type=goal_type)
        for i in range(len(r.error_codes)):
            error_code = r.error_codes[i]
            error_message = r.error_messages[i]
            if expected_error_codes is None:
                expected_error_code = MoveResult.SUCCESS
            else:
                expected_error_code = expected_error_codes[i]
            assert error_code == expected_error_code, \
                u'in goal {}; got: {}, expected: {} | error_massage: {}'.format(i, move_result_error_code(error_code),
                                                                                move_result_error_code(
                                                                                    expected_error_code),
                                                                                error_message)
        return r.trajectory

    def add_waypoint(self):
        self.wrapper.add_cmd()

    def add_json_goal(self, constraint_type, **kwargs):
        self.wrapper.set_json_goal(constraint_type, **kwargs)

    def get_result_trajectory_position(self):
        trajectory = self.get_god_map().unsafe_get_data(identifier.trajectory)
        trajectory2 = []
        for t, p in trajectory._points.items():
            trajectory2.append(
                {joint_name: js.position
                 for joint_name, js in p.items()})
        return trajectory2

    def get_result_trajectory_velocity(self):
        trajectory = self.get_god_map().get_data(identifier.trajectory)
        trajectory2 = []
        for t, p in trajectory._points.items():
            trajectory2.append(
                {joint_name: js.velocity
                 for joint_name, js in p.items()})
        return trajectory2

    def are_joint_limits_violated(self):
        controllable_joints = self.get_robot().get_movable_joints()
        trajectory_pos = self.get_result_trajectory_position()
        trajectory_vel = self.get_result_trajectory_velocity()

        for joint in controllable_joints:
            joint_limits = self.get_robot().get_joint_limits(joint)
            vel_limit = self.get_robot().get_joint_velocity_limit(joint)
            trajectory_pos_joint = [p[joint] for p in trajectory_pos]
            trajectory_vel_joint = [p[joint] for p in trajectory_vel]
            if any(
                    round(p, 7) < joint_limits[0]
                    and round(p, 7) > joint_limits[1]
                    for p in trajectory_pos_joint):
                return True
            if any(
                    round(p, 7) < vel_limit and round(p, 7) > vel_limit
                    for p in trajectory_vel_joint):
                return True

        return False

    #
    # BULLET WORLD #####################################################################################################
    #
    def get_world(self):
        """
        :rtype: PyBulletWorld
        """
        return self.get_god_map().get_data(world)

    def clear_world(self):
        assert self.wrapper.clear_world(
        ).error_codes == UpdateWorldResponse.SUCCESS
        assert len(self.get_world().get_object_names()) == 0
        assert len(self.wrapper.get_object_names().object_names) == 0
        # assert len(self.get_robot().get_attached_objects()) == 0
        # assert self.get_world().has_object(u'plane')

    def remove_object(self,
                      name,
                      expected_response=UpdateWorldResponse.SUCCESS):
        r = self.wrapper.remove_object(name)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        assert not self.get_world().has_object(name)
        assert not name in self.wrapper.get_object_names().object_names

    def detach_object(self,
                      name,
                      expected_response=UpdateWorldResponse.SUCCESS):
        if expected_response == UpdateWorldResponse.SUCCESS:
            p = self.get_robot().get_fk_pose(self.get_robot().get_root(), name)
            p = transform_pose(u'map', p)
            assert name in self.wrapper.get_attached_objects().object_names, \
                'there is no attached object named {}'.format(name)
        r = self.wrapper.detach_object(name)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        if expected_response == UpdateWorldResponse.SUCCESS:
            assert self.get_world().has_object(name)
            assert not name in self.wrapper.get_attached_objects(
            ).object_names, 'the object was not detached'
            compare_poses(self.get_world().get_object(name).base_pose,
                          p.pose,
                          decimal=2)

    def add_box(self,
                name=u'box',
                size=(1, 1, 1),
                pose=None,
                expected_response=UpdateWorldResponse.SUCCESS):
        r = self.wrapper.add_box(name, size, pose=pose)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        p = transform_pose(u'map', pose)
        o_p = self.get_world().get_object(name).base_pose
        assert self.get_world().has_object(name)
        compare_poses(p.pose, o_p)
        assert name in self.wrapper.get_object_names().object_names
        compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose)

    def add_sphere(self, name=u'sphere', size=1, pose=None):
        r = self.wrapper.add_sphere(name=name, size=size, pose=pose)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)
        assert name in self.wrapper.get_object_names().object_names
        o_p = self.get_world().get_object(name).base_pose
        compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose)

    def add_cylinder(self, name=u'cylinder', size=[1, 1], pose=None):
        r = self.wrapper.add_cylinder(name=name,
                                      height=size[0],
                                      radius=size[1],
                                      pose=pose)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)
        assert name in self.wrapper.get_object_names().object_names

    def add_mesh(self,
                 name=u'cylinder',
                 path=u'',
                 pose=None,
                 expected_error=UpdateWorldResponse.SUCCESS):
        r = self.wrapper.add_mesh(name=name, mesh=path, pose=pose)
        assert r.error_codes == expected_error, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_error))
        if expected_error == UpdateWorldResponse.SUCCESS:
            assert self.get_world().has_object(name)
            assert name in self.wrapper.get_object_names().object_names
            o_p = self.get_world().get_object(name).base_pose
            compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose)
        else:
            assert not self.get_world().has_object(name)
            assert name not in self.wrapper.get_object_names().object_names

    def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
        r = self.wrapper.add_urdf(name,
                                  urdf,
                                  pose,
                                  js_topic,
                                  set_js_topic=set_js_topic)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)
        assert name in self.wrapper.get_object_names().object_names

    def add_CollisionShape(self,
                           name,
                           path,
                           pose,
                           js_topic=u'',
                           set_js_topic=None):
        r = self.wrapper.add_urdf(name,
                                  urdf,
                                  pose,
                                  js_topic,
                                  set_js_topic=set_js_topic)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)
        assert name in self.wrapper.get_object_names().object_names

    def allow_all_collisions(self):
        self.wrapper.allow_all_collisions()

    def avoid_all_collisions(self, distance=0.5):
        self.wrapper.avoid_all_collisions(distance)
        self.loop_once()

    def allow_self_collision(self):
        self.wrapper.allow_self_collision()

    def avoid_self_collision(self):
        self.wrapper.avoid_self_collision()

    def add_collision_entries(self, collisions_entries):
        self.wrapper.set_collision_entries(collisions_entries)

    def allow_collision(self, robot_links, body_b, link_bs):
        ces = []
        ces.append(
            CollisionEntry(type=CollisionEntry.ALLOW_COLLISION,
                           robot_links=robot_links,
                           body_b=body_b,
                           link_bs=link_bs))
        self.add_collision_entries(ces)

    def avoid_collision(self, robot_links, body_b, link_bs, min_dist):
        ces = []
        ces.append(
            CollisionEntry(type=CollisionEntry.AVOID_COLLISION,
                           robot_links=robot_links,
                           body_b=body_b,
                           link_bs=link_bs,
                           min_dist=min_dist))
        self.add_collision_entries(ces)

    def attach_box(self,
                   name=u'box',
                   size=None,
                   frame_id=None,
                   position=None,
                   orientation=None,
                   pose=None,
                   expected_response=UpdateWorldResponse.SUCCESS):
        scm = self.get_robot().get_self_collision_matrix()
        if pose is None:
            expected_pose = PoseStamped()
            expected_pose.header.frame_id = frame_id
            expected_pose.pose.position = Point(*position)
            if orientation:
                expected_pose.pose.orientation = Quaternion(*orientation)
            else:
                expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
        else:
            expected_pose = deepcopy(pose)
        r = self.wrapper.attach_box(name, size, frame_id, position,
                                    orientation, pose)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        if expected_response == UpdateWorldResponse.SUCCESS:
            self.wait_for_synced()
            assert name in self.get_controllable_links()
            assert not self.get_world().has_object(name)
            assert not name in self.wrapper.get_object_names().object_names
            assert name in self.wrapper.get_attached_objects(
            ).object_names, 'object {} was not attached'
            assert scm.difference(
                self.get_robot().get_self_collision_matrix()) == set()
            assert len(scm) < len(self.get_robot().get_self_collision_matrix())
            compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
        self.loop_once()

    def attach_cylinder(self,
                        name=u'cylinder',
                        height=1,
                        radius=1,
                        frame_id=None,
                        position=None,
                        orientation=None,
                        expected_response=UpdateWorldResponse.SUCCESS):
        scm = self.get_robot().get_self_collision_matrix()
        expected_pose = PoseStamped()
        expected_pose.header.frame_id = frame_id
        expected_pose.pose.position = Point(*position)
        if orientation:
            expected_pose.pose.orientation = Quaternion(*orientation)
        else:
            expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
        r = self.wrapper.attach_cylinder(name, height, radius, frame_id,
                                         position, orientation)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        if expected_response == UpdateWorldResponse.SUCCESS:
            self.wait_for_synced()
            assert name in self.get_controllable_links()
            assert not self.get_world().has_object(name)
            assert not name in self.wrapper.get_object_names().object_names
            assert name in self.wrapper.get_attached_objects().object_names
            assert scm.difference(
                self.get_robot().get_self_collision_matrix()) == set()
            assert len(scm) < len(self.get_robot().get_self_collision_matrix())
            compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
        self.loop_once()

    def attach_existing(self,
                        name=u'box',
                        frame_id=None,
                        expected_response=UpdateWorldResponse.SUCCESS):
        scm = self.get_robot().get_self_collision_matrix()
        r = self.wrapper.attach_object(name, frame_id)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        self.wait_for_synced()
        assert name in self.get_controllable_links()
        assert not self.get_world().has_object(name)
        assert not name in self.wrapper.get_object_names().object_names
        assert name in self.wrapper.get_attached_objects().object_names
        assert scm.difference(
            self.get_robot().get_self_collision_matrix()) == set()
        assert len(scm) < len(self.get_robot().get_self_collision_matrix())
        self.loop_once()

    def get_external_collisions(self, link, distance_threshold):
        """
        :param distance_threshold:
        :rtype: list
        """
        collision_goals = [
            CollisionEntry(type=CollisionEntry.AVOID_ALL_COLLISIONS,
                           min_dist=distance_threshold)
        ]
        collision_matrix = self.get_world(
        ).collision_goals_to_collision_matrix(collision_goals,
                                              defaultdict(lambda: 0.3))
        collisions = self.get_world().check_collisions(collision_matrix)
        controlled_parent_joint = self.get_robot().get_controlled_parent_joint(
            link)
        controlled_parent_link = self.get_robot().get_child_link_of_joint(
            controlled_parent_joint)
        collision_list = collisions.get_external_collisions(
            controlled_parent_link)
        for key, self_collisions in collisions.self_collisions.items():
            if controlled_parent_link in key:
                collision_list.update(self_collisions)
        return collision_list

    def check_cpi_geq(self, links, distance_threshold):
        for link in links:
            collisions = self.get_external_collisions(link, distance_threshold)
            assert collisions[0].get_contact_distance() >= distance_threshold, \
                u'distance for {}: {} >= {}'.format(link,
                                                    collisions[0].get_contact_distance(),
                                                    distance_threshold)

    def check_cpi_leq(self, links, distance_threshold):
        for link in links:
            collisions = self.get_external_collisions(link, distance_threshold)
            assert collisions[0].get_contact_distance() <= distance_threshold, \
                u'distance for {}: {} <= {}'.format(link,
                                                    collisions[0].get_contact_distance(),
                                                    distance_threshold)

    def move_base(self, goal_pose):
        """
        :type goal_pose: PoseStamped
        """
        self.simple_base_pose_pub.publish(goal_pose)
        rospy.sleep(.07)
        self.wait_for_synced()
        current_pose = self.get_robot().get_base_pose()
        goal_pose = transform_pose(u'map', goal_pose)
        compare_poses(goal_pose.pose, current_pose.pose, decimal=1)

    def reset_base(self):
        p = PoseStamped()
        p.header.frame_id = self.map
        p.pose.orientation.w = 1
        self.teleport_base(p)
Ejemplo n.º 2
0
class GiskardTestWrapper(object):
    def __init__(self, config_file):
        with open(get_ros_pkg_path(u'giskardpy') + u'/config/' +
                  config_file) as f:
            config = yaml.load(f)
        rospy.set_param(u'~', config)
        rospy.set_param(u'~path_to_data_folder', u'tmp_data/')
        rospy.set_param(u'~enable_gui', False)

        self.sub_result = rospy.Subscriber(u'/giskardpy/command/result',
                                           MoveActionResult,
                                           self.cb,
                                           queue_size=100)

        self.tree = grow_tree()
        self.loop_once()
        # rospy.sleep(1)
        self.wrapper = GiskardWrapper(ns=u'tests')
        self.results = Queue(100)
        self.default_root = self.get_robot().get_root()
        self.map = u'map'
        self.simple_base_pose_pub = rospy.Publisher(u'/move_base_simple/goal',
                                                    PoseStamped,
                                                    queue_size=10)
        self.set_base = rospy.ServiceProxy(u'/base_simulator/set_joint_states',
                                           SetJointState)
        self.tick_rate = 10

        def create_publisher(topic):
            p = rospy.Publisher(topic, JointState, queue_size=10)
            rospy.sleep(.2)
            return p

        self.joint_state_publisher = KeyDefaultDict(create_publisher)
        # rospy.sleep(1)

    def wait_for_synced(self):
        sleeper = rospy.Rate(self.tick_rate)
        self.loop_once()
        # while self.tree.tip().name != u'has goal':
        #     self.loop_once()
        #     sleeper.sleep()
        # self.loop_once()
        # while self.tree.tip().name != u'has goal':
        #     self.loop_once()
        #     sleeper.sleep()

    def get_robot(self):
        """
        :rtype: Robot
        """
        return self.get_god_map().safe_get_data(robot)

    def get_god_map(self):
        """
        :rtype: giskardpy.god_map.GodMap
        """
        return Blackboard().god_map

    def cb(self, msg):
        """
        :type msg: MoveActionResult
        """
        self.results.put(msg.result)

    def loop_once(self):
        self.tree.tick()

    def get_controlled_joint_names(self):
        """
        :rtype: dict
        """
        return self.get_robot().controlled_joints

    def get_controllable_links(self):
        return self.get_robot().get_controlled_links()

    def get_current_joint_state(self):
        """
        :rtype: JointState
        """
        return rospy.wait_for_message(u'joint_states', JointState)

    def tear_down(self):
        rospy.sleep(1)
        logging.loginfo(u'stopping plugins')

    def set_object_joint_state(self, object_name, joint_state, topic=None):
        if topic is None:
            self.wrapper.set_object_joint_state(object_name, joint_state)
        else:
            self.joint_state_publisher[topic].publish(
                dict_to_joint_states(joint_state))
            rospy.sleep(.5)

        self.wait_for_synced()
        current_js = self.get_world().get_object(object_name).joint_state
        for joint_name, state in joint_state.items():
            np.testing.assert_almost_equal(current_js[joint_name].position,
                                           state, 2)

    def set_kitchen_js(self, joint_state, object_name=u'kitchen'):
        self.set_object_joint_state(object_name,
                                    joint_state,
                                    topic=u'/kitchen/cram_joint_states')

    #
    # JOINT GOAL STUFF #################################################################################################
    #

    def compare_joint_state(self, current_js, goal_js, decimal=2):
        """
        :type current_js: dict
        :type goal_js: dict
        :type decimal: int
        """
        joint_names = set(current_js.keys()).intersection(set(goal_js.keys()))
        for joint_name in joint_names:
            goal = goal_js[joint_name]
            current = current_js[joint_name]
            if self.get_robot().is_joint_continuous(joint_name):
                np.testing.assert_almost_equal(shortest_angular_distance(
                    goal, current),
                                               0,
                                               decimal=decimal)
            else:
                np.testing.assert_almost_equal(
                    current,
                    goal,
                    decimal,
                    err_msg=u'{} at {} insteand of {}'.format(
                        joint_name, current, goal))

    def set_joint_goal(self, js):
        """
        :rtype js: dict
        """
        self.wrapper.set_joint_goal(js)

    def check_joint_state(self, expected, decimal=2):
        current_joint_state = to_joint_state_dict2(
            self.get_current_joint_state())
        self.compare_joint_state(current_joint_state,
                                 expected,
                                 decimal=decimal)

    def send_and_check_joint_goal(self, goal, decimal=2):
        """
        :type goal: dict
        """
        self.set_joint_goal(goal)
        self.send_and_check_goal()
        self.check_joint_state(goal, decimal=decimal)

    #
    # CART GOAL STUFF ##################################################################################################
    #
    def teleport_base(self, goal_pose):
        goal_pose = transform_pose(self.default_root, goal_pose)
        js = {
            u'odom_x_joint':
            goal_pose.pose.position.x,
            u'odom_y_joint':
            goal_pose.pose.position.y,
            u'odom_z_joint':
            rotation_from_matrix(
                quaternion_matrix([
                    goal_pose.pose.orientation.x, goal_pose.pose.orientation.y,
                    goal_pose.pose.orientation.z, goal_pose.pose.orientation.w
                ]))[0]
        }
        goal = SetJointStateRequest()
        goal.state = dict_to_joint_states(js)
        self.set_base.call(goal)

    def keep_position(self, tip, root=None):
        if root is None:
            root = self.default_root
        goal = PoseStamped()
        goal.header.frame_id = tip
        goal.pose.orientation.w = 1
        self.set_cart_goal(goal, tip, root)

    def keep_orientation(self, tip, root=None):
        goal = PoseStamped()
        goal.header.frame_id = tip
        goal.pose.orientation.w = 1
        self.set_rotation_goal(goal, tip, root)

    def align_planes(self, tip, tip_normal, root=None, root_normal=None):
        self.wrapper.align_planes(tip, tip_normal, root, root_normal)

    def set_rotation_goal(self, goal_pose, tip, root=None):
        if not root:
            root = self.default_root
        self.wrapper.set_rotation_goal(root, tip, goal_pose)

    def set_translation_goal(self, goal_pose, tip, root=None):
        if not root:
            root = self.default_root
        self.wrapper.set_translation_goal(root, tip, goal_pose)

    def set_cart_goal(self, goal_pose, tip, root=None):
        if not root:
            root = self.default_root
        self.wrapper.set_cart_goal(root, tip, goal_pose)

    def set_and_check_cart_goal(self,
                                goal_pose,
                                tip,
                                root=None,
                                expected_error_code=MoveResult.SUCCESS):
        goal_pose_in_map = transform_pose(u'map', goal_pose)
        self.set_cart_goal(goal_pose, tip, root)
        self.loop_once()
        self.send_and_check_goal(expected_error_code)
        self.loop_once()
        self.check_cart_goal(tip, goal_pose_in_map)

    def check_cart_goal(self, tip, goal_pose):
        goal_in_base = transform_pose(u'map', goal_pose)
        current_pose = lookup_pose(u'map', tip)
        np.testing.assert_array_almost_equal(
            msg_to_list(goal_in_base.pose.position),
            msg_to_list(current_pose.pose.position),
            decimal=2)

        try:
            np.testing.assert_array_almost_equal(
                msg_to_list(goal_in_base.pose.orientation),
                msg_to_list(current_pose.pose.orientation),
                decimal=2)
        except AssertionError:
            np.testing.assert_array_almost_equal(
                msg_to_list(goal_in_base.pose.orientation),
                -np.array(msg_to_list(current_pose.pose.orientation)),
                decimal=2)

    #
    # GENERAL GOAL STUFF ###############################################################################################
    #
    def get_as(self):
        return Blackboard().get(u'giskardpy/command')

    def send_goal(self, goal=None, execute=True):
        """
        :rtype: MoveResult
        """
        if goal is None:
            goal = MoveActionGoal()
            goal.goal = self.wrapper._get_goal()
            if execute:
                goal.goal.type = MoveGoal.PLAN_AND_EXECUTE
            else:
                goal.goal.type = MoveGoal.PLAN_ONLY
        i = 0
        self.loop_once()
        t1 = Thread(
            target=self.get_as()._as.action_server.internal_goal_callback,
            args=(goal, ))
        self.loop_once()
        t1.start()
        sleeper = rospy.Rate(self.tick_rate)
        while self.results.empty():
            self.loop_once()
            sleeper.sleep()
            i += 1
        t1.join()
        self.loop_once()
        result = self.results.get()
        return result

    def send_and_check_goal(self,
                            expected_error_code=MoveResult.SUCCESS,
                            execute=True):
        r = self.send_goal(execute=execute)
        assert r.error_code == expected_error_code, \
            u'got: {}, expected: {}'.format(move_result_error_code(r.error_code),
                                            move_result_error_code(expected_error_code))

    def add_waypoint(self):
        self.wrapper.add_cmd()

    def add_json_goal(self, constraint_type, **kwargs):
        self.wrapper.set_json_goal(constraint_type, **kwargs)

    def get_trajectory_msg(self):
        trajectory = self.get_god_map().get_data(identifier.trajectory)
        trajectory2 = []
        for t, p in trajectory._points.items():
            trajectory2.append(
                {joint_name: js.position
                 for joint_name, js in p.items()})
        return trajectory2

    #
    # BULLET WORLD #####################################################################################################
    #
    def get_world(self):
        """
        :rtype: PyBulletWorld
        """
        return self.get_god_map().safe_get_data(world)

    def clear_world(self):
        assert self.wrapper.clear_world(
        ).error_codes == UpdateWorldResponse.SUCCESS
        assert len(self.get_world().get_object_names()) == 0
        # assert len(self.get_robot().get_attached_objects()) == 0
        # assert self.get_world().has_object(u'plane')

    def remove_object(self,
                      name,
                      expected_response=UpdateWorldResponse.SUCCESS):
        r = self.wrapper.remove_object(name)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        assert not self.get_world().has_object(name)

    def detach_object(self,
                      name,
                      expected_response=UpdateWorldResponse.SUCCESS):
        if expected_response == UpdateWorldResponse.SUCCESS:
            p = self.get_robot().get_fk_pose(self.get_robot().get_root(), name)
            p = transform_pose(u'map', p)
        r = self.wrapper.detach_object(name)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        if expected_response == UpdateWorldResponse.SUCCESS:
            assert self.get_world().has_object(name)
            compare_poses(self.get_world().get_object(name).base_pose,
                          p.pose,
                          decimal=2)

    def add_box(self,
                name=u'box',
                size=(1, 1, 1),
                pose=None,
                expected_response=UpdateWorldResponse.SUCCESS):
        r = self.wrapper.add_box(name, size, pose=pose)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        p = transform_pose(u'map', pose)
        o_p = self.get_world().get_object(name).base_pose
        assert self.get_world().has_object(name)
        compare_poses(p.pose, o_p)

    def add_sphere(self, name=u'sphere', size=1, pose=None):
        r = self.wrapper.add_sphere(name=name, size=size, pose=pose)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)

    def add_cylinder(self, name=u'cylinder', size=[1, 1], pose=None):
        r = self.wrapper.add_cylinder(name=name, size=size, pose=pose)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)

    def add_urdf(self, name, urdf, pose, js_topic):
        r = self.wrapper.add_urdf(name, urdf, pose, js_topic)
        assert r.error_codes == UpdateWorldResponse.SUCCESS, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(UpdateWorldResponse.SUCCESS))
        assert self.get_world().has_object(name)

    def allow_all_collisions(self):
        self.wrapper.allow_all_collisions()

    def avoid_all_collisions(self, distance=0.5):
        self.wrapper.avoid_all_collisions(distance)
        self.loop_once()

    def enable_self_collision(self):
        pass

    def allow_self_collision(self):
        self.wrapper.allow_self_collision()

    def add_collision_entries(self, collisions_entries):
        self.wrapper.set_collision_entries(collisions_entries)

    def allow_collision(self, robot_links, body_b, link_bs):
        ces = []
        ces.append(
            CollisionEntry(type=CollisionEntry.ALLOW_COLLISION,
                           robot_links=robot_links,
                           body_b=body_b,
                           link_bs=link_bs))
        self.add_collision_entries(ces)

    def avoid_collision(self, robot_links, body_b, link_bs, min_dist):
        ces = []
        ces.append(
            CollisionEntry(type=CollisionEntry.AVOID_COLLISION,
                           robot_links=robot_links,
                           body_b=body_b,
                           link_bs=link_bs,
                           min_dist=min_dist))
        self.add_collision_entries(ces)

    def attach_box(self,
                   name=u'box',
                   size=None,
                   frame_id=None,
                   position=None,
                   orientation=None,
                   expected_response=UpdateWorldResponse.SUCCESS):
        scm = self.get_robot().get_self_collision_matrix()
        expected_pose = PoseStamped()
        expected_pose.header.frame_id = frame_id
        expected_pose.pose.position = Point(*position)
        if orientation:
            expected_pose.pose.orientation = Quaternion(*orientation)
        else:
            expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
        r = self.wrapper.attach_box(name, size, frame_id, position,
                                    orientation)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        if expected_response == UpdateWorldResponse.SUCCESS:
            self.wait_for_synced()
            assert name in self.get_controllable_links()
            assert not self.get_world().has_object(name)
            assert scm.difference(
                self.get_robot().get_self_collision_matrix()) == set()
            assert len(scm) < len(self.get_robot().get_self_collision_matrix())
            compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
        self.loop_once()

    def attach_cylinder(self,
                        name=u'cylinder',
                        height=1,
                        radius=1,
                        frame_id=None,
                        position=None,
                        orientation=None,
                        expected_response=UpdateWorldResponse.SUCCESS):
        scm = self.get_robot().get_self_collision_matrix()
        expected_pose = PoseStamped()
        expected_pose.header.frame_id = frame_id
        expected_pose.pose.position = Point(*position)
        if orientation:
            expected_pose.pose.orientation = Quaternion(*orientation)
        else:
            expected_pose.pose.orientation = Quaternion(0, 0, 0, 1)
        r = self.wrapper.attach_cylinder(name, height, radius, frame_id,
                                         position, orientation)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        if expected_response == UpdateWorldResponse.SUCCESS:
            self.wait_for_synced()
            assert name in self.get_controllable_links()
            assert not self.get_world().has_object(name)
            assert scm.difference(
                self.get_robot().get_self_collision_matrix()) == set()
            assert len(scm) < len(self.get_robot().get_self_collision_matrix())
            compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose)
        self.loop_once()

    def attach_existing(self,
                        name=u'box',
                        frame_id=None,
                        expected_response=UpdateWorldResponse.SUCCESS):
        scm = self.get_robot().get_self_collision_matrix()
        r = self.wrapper.attach_object(name, frame_id)
        assert r.error_codes == expected_response, \
            u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes),
                                            update_world_error_code(expected_response))
        self.wait_for_synced()
        assert name in self.get_controllable_links()
        assert not self.get_world().has_object(name)
        assert scm.difference(
            self.get_robot().get_self_collision_matrix()) == set()
        assert len(scm) < len(self.get_robot().get_self_collision_matrix())
        self.loop_once()

    def get_external_collisions(self, link, distance_threshold):
        """
        :param distance_threshold:
        :rtype: list
        """
        collision_goals = [
            CollisionEntry(type=CollisionEntry.AVOID_ALL_COLLISIONS,
                           min_dist=distance_threshold)
        ]
        collision_matrix = self.get_world(
        ).collision_goals_to_collision_matrix(
            collision_goals,
            self.get_god_map().safe_get_data(identifier.distance_thresholds))
        collisions = self.get_world().check_collisions(collision_matrix)
        collisions = self.get_world().transform_contact_info(collisions)
        collision_list = collisions.external_collision[
            self.get_robot().get_movable_parent_joint(link)]
        for key, self_collisions in collisions.self_collisions.items():
            if link in key:
                collision_list.update(self_collisions)
        return collision_list

    def check_cpi_geq(self, links, distance_threshold):
        for link in links:
            collisions = self.get_external_collisions(link, distance_threshold)
            assert collisions[0].contact_distance >= distance_threshold

    def check_cpi_leq(self, links, distance_threshold):
        for link in links:
            collisions = self.get_external_collisions(link, distance_threshold)
            assert collisions[0].contact_distance <= distance_threshold

    def move_base(self, goal_pose):
        """
        :type goal_pose: PoseStamped
        """
        self.simple_base_pose_pub.publish(goal_pose)
        rospy.sleep(.07)
        self.wait_for_synced()
        current_pose = self.get_robot().get_base_pose()
        goal_pose = transform_pose(u'map', goal_pose)
        compare_poses(goal_pose.pose, current_pose.pose, decimal=1)

    def reset_base(self):
        p = PoseStamped()
        p.header.frame_id = self.map
        p.pose.orientation.w = 1
        self.teleport_base(p)
class MotionTaskWithConstraint:
    """
    the class helps to select the necessary constraints, to select the gripper and to check the orientation of the base.
    """
    # list of Gripper
    _list_grippers = []
    # Torso of robot
    _torso = ""
    # Reference frame to basefootprint
    _origin = ""
    # goal frame
    _goal = ""

    def __init__(self):
        print("MotionTaskWithConstraint is initialized !")
        self._giskard = GiskardWrapper()

        rospy.logout("- Set pose kitchen")
        kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
        kitchen_pose.header.frame_id = "map"
        # setup kitchen
        rospy.logout("- Get urdf")
        self.urdf = rospy.get_param("kitchen_description")
        self.parsed_urdf = URDF.from_xml_string(self.urdf)
        self.config_file = None

        rospy.logout("- clear urdf and add kitchen urdf")
        self._giskard.clear_world()
        self._giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")

    def set_gripper_for_real_roboter(self):
        # setup gripper as actionlib
        self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction)
        self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction)
        self.rgripper.wait_for_server()
        self.lgripper.wait_for_server()

    def r_action_gripper(self, value=0):
        goal = Pr2GripperCommandGoal() #GripperCommandGoal()
        goal.command.position = value
        goal.command.max_effort = 40
        self.rgripper.send_goal_and_wait(goal)

    def l_action_gripper(self, value=0):
        goal = Pr2GripperCommandGoal() #GripperCommandGoal()
        goal.command.position = value
        goal.command.max_effort = 40
        self.lgripper.send_goal_and_wait(goal)

    def execute_open_translational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
        self.r_action_gripper(0.08)
        self.l_action_gripper(0.08)
        self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
        self._giskard.allow_all_collisions()
        self._giskard.plan_and_execute()
        self.r_action_gripper(0.012)
        self.l_action_gripper(0.012)
        self._giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action)
        self._giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_object_joint_state(object_name='kitchen',
                                             joint_states={joint: joint_value})

    def execute_close_translational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
        self.r_action_gripper(0.08)
        self.l_action_gripper(0.08)
        self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
        self._giskard.allow_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action)
        self._giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_object_joint_state(object_name='kitchen',
                                             joint_states={joint: joint_value})

    def execute_open_circular_motion_on_real_robot(self, goal, body, action, joint, joint_value):
        self.r_action_gripper(0.08)
        self.l_action_gripper(0.08)
        self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
        self._giskard.allow_all_collisions()
        self._giskard.plan_and_execute()
        self.r_action_gripper(0.012)
        self.l_action_gripper(0.012)
        self._giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action)
        self._giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_object_joint_state(object_name='kitchen',
                                             joint_states={joint: joint_value})

    def execute_close_circular_motion_on_real_robot(self, goal, body, action, joint, joint_value):
        self.r_action_gripper(0.08)
        self.l_action_gripper(0.08)
        self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
        self._giskard.allow_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action)
        self._giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_object_joint_state(object_name='kitchen',
                                             joint_states={joint: joint_value})

    def execute_open_rotational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
        self.r_action_gripper(0.08)
        self.l_action_gripper(0.08)
        self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
        self._giskard.allow_all_collisions()
        self._giskard.plan_and_execute()
        self.r_action_gripper(0.012)
        self.l_action_gripper(0.012)
        self._giskard.set_json_goal("TurnRotaryKnobConstraint", goal_name=goal, body_name=body, action=action)
        self._giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_object_joint_state(object_name='kitchen',
                                             joint_states={joint: joint_value})

    def execute_close_rotational_motion_on_real_robot(self, goal, body, action, joint, joint_value):
        self.r_action_gripper(0.08)
        self.l_action_gripper(0.08)
        self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)
        self._giskard.allow_all_collisions()
        self._giskard.plan_and_execute()
        self._giskard.set_json_goal("TurnRotaryKnobConstraint", goal_name=goal, body_name=body, action=action)
        self.giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self.giskard.plan_and_execute()
        self._giskard.set_object_joint_state(object_name='kitchen',
                                            joint_states={joint: joint_value})

    def setup_orientation_basis(self, basis_frame="odom_combined", torso="torso_lift_link",
                                list_grippers=["r_gripper_tool_frame", "l_gripper_tool_frame"]):
        """
        This method set the parameter for the orientations verification of the basis
        :param basis_frame: origin frame to the basis
        :type: str
        :param torso: torso of robot
        :type: str
        :param list_grippers: List of gripper
        :type: array of str
        :return:
        """
        self._list_grippers = list_grippers
        self._torso = torso
        self._origin = basis_frame

    def set_goal(self, goal=""):
        """
        set the goal in relation to which the base is to be oriented
        :param goal: goal
        :type: str
        :return:
        """
        self._goal = goal

    def update_basis_orientation(self):
        """
        This function update the orientation of the basis and get the next Gripper in short distance from goal
        :return:
        """
        x, y, r = self.get_current_base_position()
        rospy.logout("The basis is at position :")
        rospy.logout(x)
        rospy.logout(y)
        rospy.logout(r)
        # get pose torso
        pose_torso = tf_wrapper.lookup_pose(self._origin, self._torso)
        # get pose goal
        pose_goal = tf_wrapper.lookup_pose(self._origin, self._goal)
        # get list of pose of gripper
        list_pose_gripper = [tf_wrapper.lookup_pose(self._origin, p) for p in self._list_grippers]
        # get angle to G
        angle = [float(w.get_angle_casadi(self.get_x_y(pose_torso), self.get_x_y(pose_goal), self.get_x_y(p)))
                 for p in list_pose_gripper]
        # max angle G to goal
        max_angle = np.amax(angle)
        rospy.logout("Max angle is :")
        rospy.logout(max_angle)
        # update old and new orientation
        max_angle = r + max_angle
        rospy.logout("Max angle + r is :")
        rospy.logout(max_angle)
        # get selected Gripper
        gripper = np.where(np.array(angle) == np.amax(angle))
        rospy.logout("Farther away Gripper:")
        rospy.logout(gripper[0][0])
        # convert angle in axis
        q = w.quaternion_from_axis_angle([0, 0, 1], max_angle)
        # update basis orientation
        #self.set_cart_goal_for_basis(x, y, 0, q)

    #@staticmethod
    def get_x_y(self, pose):
        return [pose.pose.position.x, pose.pose.position.y, 0]

    def get_current_base_position(self):
        """
        the method check the current pose of base_footprint to odom and get it
        :return: x, y, rotation
        """
        basis = tf_wrapper.lookup_pose(self._origin, "base_footprint")
        t, r = [basis.pose.position.x, basis.pose.position.y, basis.pose.position.z], \
               [basis.pose.orientation.x, basis.pose.orientation.y, basis.pose.orientation.z,
                basis.pose.orientation.w]  # self.get_msg_translation_and_rotation(self._basis, "map")
        return self.base_position(t, r)

    def base_position(self, translation, rotation):
        """
        the method take the translation and rotation from tf_ros and calculate the rotation in euler
        :param translation: vector
        :param rotation: quaternion (from tf_ros)
        :return: vector[x, y, rotation], current base position
        """
        euler = tf.transformations.euler_from_quaternion(rotation)
        return translation[0], translation[1], euler[2]

    def set_cart_goal_for_basis(self, x, y, z, q):
        """
        this function send goal to the basis of robot
        :param x: position on x axis
        :type: float
        :param y: position on y axis
        :type: float
        :param z: position on z axis
        :type: float
        :param q: quaternion orientation
        :type: array float
        :return:
        """
        # take base_footprint
        poseStamp = lookup_pose(self._origin, "base_footprint")
        poseStamp.header.frame_id = self._origin
        # set goal pose
        poseStamp.pose.position.x = x
        poseStamp.pose.position.y = y
        poseStamp.pose.position.z = z
        poseStamp.pose.orientation.x = q[0]
        poseStamp.pose.orientation.y = q[1]
        poseStamp.pose.orientation.z = q[2]
        poseStamp.pose.orientation.w = q[3]
        self._giskard.set_cart_goal(self._origin, "base_footprint", poseStamp)
        self._giskard.avoid_all_collisions()
        self._giskard.plan_and_execute()

    def reset_kitchen(self, list_joints):
        for j in list_joints:
            self._giskard.set_object_joint_state(object_name='kitchen',
                                                 joint_states={j: 0.0})
Ejemplo n.º 4
0
class Gripper:

    def __init__(self):
        """
        this class controls and performs mvt of gripper
        """
        self.giskard = GiskardWrapper()
        rospy.logout("--> Set kitchen/world in Giskard")

        rospy.logout("- Set pose kitchen")
        kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world")
        kitchen_pose.header.frame_id = "map"
        # setup kitchen
        rospy.logout("- Get urdf")
        self.urdf = rospy.get_param("kitchen_description")
        self.parsed_urdf = URDF.from_xml_string(self.urdf)
        self.config_file = None

        rospy.logout("- clear urdf and add kitchen urdf")
        self.giskard.clear_world()
        self.giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")

        # setup gripper as service
        rospy.logout("- Set right and left Gripper service proxy")
        self.l_gripper_service = rospy.ServiceProxy('/l_gripper_simulator/set_joint_states', SetJointState)
        self.r_gripper_service = rospy.ServiceProxy('/r_gripper_simulator/set_joint_states', SetJointState)
        # setup gripper as actionlib
        # self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction)
        # self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction)
        # self.rgripper.wait_for_server()
        # self.lgripper.wait_for_server()

        rospy.logout("--> Gripper are ready for every task.")

    def r_action_gripper(self, value=0):
        goal = GripperCommandGoal()
        goal.command.position = value
        goal.command.max_effort = 10
        self.rgripper.send_goal_and_wait(goal)

    def l_action_gripper(self, value=0):
        goal = GripperCommandGoal()
        goal.command.position = value
        goal.command.max_effort = 10
        self.lgripper.send_goal_and_wait(goal)

    def some_mvt(self, goal, body):
        """
        execute of simple movement of position and rotation with the grippers
        :param goal: goal name
        :type: str
        :param body: End Link, l_gripper or r_gripper
        :type: str
        :param axis: the axis on which the object must be caught, maybe 'x', 'y' or 'z'
        :type: str
        :return:
        """
        # self.giskard.set_json_goal("MoveWithConstraint", goal_name=goal, body_name=body)
        self.giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body)  # movetopose
        # self.giskard.set_json_goal("CartesianPoseUpdate", root_link="odom_combined", tip_link=body, goal_name=goal)
        # self.giskard.avoid_all_collisions()
        self.giskard.allow_all_collisions()
        self.giskard.plan_and_execute()

    def open_or_close_with_translation(self, goal, body, action):
        """
        execute simple translation of gripper with something
        :param goal: goal name (Frame)
        :type: str
        :param body: End Link, l_gripper or r_gripper
        :type: str
        :param action: percentage of movement
        :type: float
        :return:
        """
        # self.giskard.set_json_goal("TranslationalAngularConstraint", goal_name=goal, body_name=body, action=action)
        self.giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action)  #  openclosedrawerconstraint
        #self.giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action)  #  openclosedoorconstraint
        self.giskard.allow_all_collisions()
        # self.giskard.avoid_all_collisions()
        self.giskard.plan_and_execute()

    def do_rotational_mvt(self, goal, body, action):
        """
        Execute rotational movement with the gripper(body)
        :param goal: goal name
        :type: str
        :param body: End Link, l/r Gripper
        :str:  str
        :param action: percentage of movement
        :rtype: float
        :return:
        """
        self.giskard.set_json_goal("RotationalConstraint", goal_name=goal, body_name=body, action=action)
        # self.giskard.allow_collision(robot_links=(body), link_bs=(goal))
        self.giskard.allow_all_collisions()
        self.giskard.plan_and_execute()

    def mvt_l_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint):
        """
        execute mvt of left gripper
        :param l_finger_joint: value of l_gripper_l_finger_joint
        :type: float
        :param r_finger_joint: value of l_gripper_r_finger_joint
        :type: float
        :param l_finger_tip_joint: value of l_gripper_l_finger_tip_joint
        :type: float
        :param r_finger_tip_joint: value of l_gripper_r_finger_tip_joint
        :type: float
        :return:
        """
        rospy.logout('Start mvt L gripper')
        gripper_request = SetJointStateRequest()
        gripper_request.state.name = ['l_gripper_l_finger_joint', 'l_gripper_r_finger_joint',
                                      'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_tip_joint']
        gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint]
        gripper_request.state.velocity = [0, 0, 0, 0]
        gripper_request.state.effort = [0, 0, 0, 0]
        self.l_gripper_service.call(gripper_request)
        rospy.logout('End mvt L gripper')

    def mvt_r_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint):
        """
        execute mvt of right gripper
        :param l_finger_joint: value of r_gripper_l_finger_joint
        :type: float
        :param r_finger_joint: value of r_gripper_r_finger_joint
        :type: float
        :param l_finger_tip_joint: value of r_gripper_l_finger_tip_joint
        :type: float
        :param r_finger_tip_joint: value of r_gripper_r_finger_tip_joint
        :type: float
        :return:
        """
        rospy.logout('Start mvt R gripper')
        gripper_request = SetJointStateRequest()
        gripper_request.state.name = ['r_gripper_l_finger_joint', 'r_gripper_r_finger_joint',
                                      'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint']
        gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint]
        gripper_request.state.velocity = [0, 0, 0, 0]
        gripper_request.state.effort = [0, 0, 0, 0]
        self.r_gripper_service.call(gripper_request)
        rospy.logout('End mvt R gripper')

    def get_pose(self, source, frame_id):
        """
        the method get exactly the pose of link frame_id to source(map or odom)
        :param source: odom or map, typ string
        :param frame_id: string
        :return: PoseStamped
        """
        tf_buffer = tf2_ros.Buffer()
        tf_listener = tf2_ros.TransformListener(tf_buffer)
        # print tf_listener
        ps = PoseStamped()

        transform = tf_buffer.lookup_transform(source,
                                               frame_id,
                                               rospy.Time(0),
                                               rospy.Duration(5.0))
        # print transform

        return tf2_geometry_msgs.do_transform_pose(ps, transform)

    def info_about_joints(self, joint_name):
        """
        This method get info about some joint ( limits and axis)
        :param joint_name: name of the joint
        :type: String
        :return: joint_limit, joint_axis
        :rtype: {'effort': float, 'lower': float, 'upper': float, 'velocity': float}, array 1*3
        """
        joint_limits = {}
        joint_axis = None
        for j in self.parsed_urdf.joints:
            if joint_name == j.name:
                if j.axis != None:
                    joint_axis = j.axis
                if j.limit != None:
                    joint_limits = {'effort': j.limit.effort, 'lower': j.limit.lower,
                                    'upper': j.limit.upper, 'velocity': j.limit.velocity}
        return joint_limits, joint_axis

    def set_kitchen_goal(self, joint_name, joint_value):
        self.giskard.set_object_joint_state(object_name='kitchen',
                                            joint_states={joint_name: joint_value})