Пример #1
0
    def continuousSolve(self,
                        itrs=1000,
                        auto=False,
                        extra=[],
                        show=True,
                        quitonsolve=False):
        """ Continously solve in realtime, allowing a user to reposition
         the robot, to quickly get an intuitive idea of what works
        and doesn't. Note that TSR's based on object poses are not recalculated
        if an object is moved."""
        report = _rave.CollisionReport()
        env = self.robot.GetEnv()
        pose = Pose(self.robot)
        for k in xrange(itrs):
            pose.send()
            with self.robot:
                self.run(auto, extra)
                colcheck = env.CheckCollision(
                    self.robot,
                    report=report) and self.robot.CheckSelfCollision(
                    ) if self.bcollisions else False

            if show:
                self.goto()
                _time.sleep(.1)
            if self.solved and not colcheck and quitonsolve:
                break

        return self.solved()
Пример #2
0
class TestCollisionMap(unittest.TestCase):

    def setUp(self):
        env=Environment()
        #env.SetViewer('qtcoin')
        env.Load('test_collisionmap.env.xml')

        robot=env.GetRobots()[0]

        self.pose=Pose(robot)
        self.env=env
        self.env.SetDebugLevel(1)
        self.robot=robot

    def test_ode_self_collision(self):
        self.env.SetCollisionChecker(RaveCreateCollisionChecker(self.env,'ode'))
        self._iterate_over_joint()

    def test_pqp_self_collision(self):
        self.env.SetCollisionChecker(RaveCreateCollisionChecker(self.env,'pqp'))
        self._iterate_over_joint()

    def _iterate_over_joint(self):
        self.pose['LHR']=20*pi/180
        cols=[]
        for k in range(100):
            self.pose['LHP']=k*pi/180
            self.pose.send()
            cols.append(self.robot.CheckSelfCollision())
        self.assertTrue(max(cols))
Пример #3
0
    def setUp(self):
        env=Environment()
        #env.SetViewer('qtcoin')
        env.Load('test_collisionmap.env.xml')

        robot=env.GetRobots()[0]

        self.pose=Pose(robot)
        self.env=env
        self.env.SetDebugLevel(1)
        self.robot=robot
Пример #4
0
    def continuousSolve(self,itrs=1000,auto=False,extra=[],show=True,quitonsolve=False):
        """ Continously solve in realtime, allowing a user to reposition
         the robot, to quickly get an intuitive idea of what works
        and doesn't. Note that TSR's based on object poses are not recalculated
        if an object is moved."""
        report=_rave.CollisionReport()
        env=self.robot.GetEnv()
        pose=Pose(self.robot)
        for k in xrange(itrs):
            pose.send()
            with self.robot:
                self.run(auto,extra)
                colcheck = env.CheckCollision(self.robot,report=report) and self.robot.CheckSelfCollision() if self.bcollisions else False

            if show:
                self.goto()
                _time.sleep(.1)
            if self.solved and not colcheck and quitonsolve:
                break

        return self.solved()
Пример #5
0
    def set_ikguess_pose(self, robot=None, pose=None):
        """Use the current pose of the robot as a source for the IK guess.
        Stores the whole pose, and trims it internally to what is needed for
        the problem. Use it like this:

            with robot:
                #Set robot pose to Ik guess position
                ...
                mycbirrt.set_ikguess_pose(robot)
                or
                mycbirrt.set_ikguess_pose(pose)

        """
        if self.robot is None and robot is not None:
            self.ikpose = Pose(robot)
            self.robot = robot
        elif pose is not None:
            self.ikpose = pose
        else:
            raise ValueError(
                'Robot is not provided internally or in arguments')