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