common_test_openrave.py | common_test_openrave.py | |||
---|---|---|---|---|
skipping to change at line 36 | skipping to change at line 36 | |||
import os | import os | |||
import cPickle as pickle | import cPickle as pickle | |||
_multiprocess_can_split_ = True | _multiprocess_can_split_ = True | |||
g_epsilon = 1e-7 | g_epsilon = 1e-7 | |||
g_jacobianstep = 0.01 | g_jacobianstep = 0.01 | |||
g_envfiles = ['data/lab1.env.xml','data/pr2wam_test1.env.xml','data/hanoi_c omplex.env.xml'] | g_envfiles = ['data/lab1.env.xml','data/pr2wam_test1.env.xml','data/hanoi_c omplex.env.xml'] | |||
g_robotfiles = ['robots/pr2-beta-static.zae','robots/barrettsegway.robot.xm l','robots/neuronics-katana.zae','robots/pa10schunk.robot.xml','robots/barr ettwam-dual.robot.xml'] | g_robotfiles = ['robots/pr2-beta-static.zae','robots/barrettsegway.robot.xm l','robots/neuronics-katana.zae','robots/pa10schunk.robot.xml','robots/barr ettwam-dual.robot.xml'] | |||
def setup_module(module): | def setup_module(module): | |||
os.environ['OPENRAVE_DATABASE'] = os.path.join(os.getcwd(),'.openravete | dbdir = os.path.join(os.getcwd(),'.openravetest') | |||
st') # don't mess with the default OPENRAVE_DATABASE | os.environ['OPENRAVE_DATABASE'] = dbdir | |||
os.environ['OPENRAVE_HOME'] = dbdir | ||||
RaveInitialize(load_all_plugins=True, level=int32(DebugLevel.Info)|int3 2(DebugLevel.VerifyPlans)) | RaveInitialize(load_all_plugins=True, level=int32(DebugLevel.Info)|int3 2(DebugLevel.VerifyPlans)) | |||
assert(os.path.samefile(RaveGetHomeDirectory(),dbdir)) | ||||
def teardown_module(module): | def teardown_module(module): | |||
RaveDestroy() | RaveDestroy() | |||
def transdist(list0,list1): | def transdist(list0,list1): | |||
assert(len(list0)==len(list1)) | assert(len(list0)==len(list1)) | |||
return sum([sum(abs(item0-item1)) for item0, item1 in izip(list0,list1) ]) | return sum([sum(abs(item0-item1)) for item0, item1 in izip(list0,list1) ]) | |||
def axisangledist(axis0,axis1): | def axisangledist(axis0,axis1): | |||
return arccos(numpy.minimum(1.0,abs(dot(quatFromAxisAngle(axis0),quatFr omAxisAngle(axis1))))) | return arccos(numpy.minimum(1.0,abs(dot(quatFromAxisAngle(axis0),quatFr omAxisAngle(axis1))))) | |||
skipping to change at line 118 | skipping to change at line 121 | |||
self.env.AddRobot(robot,True) | self.env.AddRobot(robot,True) | |||
self._PreprocessRobot(robot) | self._PreprocessRobot(robot) | |||
return robot | return robot | |||
def LoadRobotData(self,*args,**kwargs): | def LoadRobotData(self,*args,**kwargs): | |||
robot=self.env.ReadRobotData(*args,**kwargs) | robot=self.env.ReadRobotData(*args,**kwargs) | |||
self.env.AddRobot(robot,True) | self.env.AddRobot(robot,True) | |||
self._PreprocessRobot(robot) | self._PreprocessRobot(robot) | |||
return robot | return robot | |||
def RunTrajectory(self,robot,traj): | ||||
assert(traj is not None) | ||||
robot.GetController().SetPath(traj) | ||||
while not robot.GetController().IsDone(): | ||||
self.env.StepSimulation(0.01) | ||||
def _PreprocessEnv(self): | def _PreprocessEnv(self): | |||
for robot in self.env.GetRobots(): | for robot in self.env.GetRobots(): | |||
self._PreprocessRobot(robot) | self._PreprocessRobot(robot) | |||
def _PreprocessRobot(self,robot): | def _PreprocessRobot(self,robot): | |||
if robot.GetController() is not None and robot.GetController().GetX MLId().lower() == 'idealcontroller': | if robot.GetController() is not None and robot.GetController().GetX MLId().lower() == 'idealcontroller': | |||
# need to throw exceptions so test fails | # need to throw exceptions so test fails | |||
robot.GetController().SendCommand('SetThrowExceptions 1') | robot.GetController().SendCommand('SetThrowExceptions 1') | |||
End of changes. 3 change blocks. | ||||
2 lines changed or deleted | 10 lines changed or added | |||
This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/ |