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()
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))
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()
