def test_get_joint(self):
""" This test creates a path in openrave format and test the class method get_joint to return a known joint.
"""
path = blade_coverage.Path(self.rays)
ind = str()
for i in range(self.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind,
len(self.robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(self.turbine.env, '')
traj.Init(cs)
for i in range(len(self.joints[:1])):
waypoint = list(self.joints[i])
waypoint.extend(list(self.joints[i]))
waypoint.extend(list(self.joints[i]))
waypoint.extend([0])
traj.Insert(traj.GetNumWaypoints(), waypoint)
path.data = [traj]
joint = path.get_joint(self.robot, 0, 0)
self.assertTrue(linalg.norm(joint - self.joints[0]) <= 1e-5,
msg='get_joint failed')
def deserialize(self, turbine, directory):
""" Method deserializes data in OpenRave format.
Args:
turbine: (@ref Turbine) is the turbine object.
directory: (str) is the relative path to the folder where to load.
Examples:
>>> path.deserialize(turbine,'my_folder')
"""
ind = str()
for i in range(turbine.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + turbine.robot.GetName() + ' ' + ind,
len(turbine.robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
directory = realpath(directory)
TRAJ = []
onlyfiles = [
f for f in listdir(directory) if isfile(join(directory, f))
]
onlyfiles.sort(key=int)
for afile in onlyfiles:
traj = RaveCreateTrajectory(turbine.env, '')
traj.Init(cs)
xml = ET.parse(open(join(directory, afile)))
traj.deserialize(ET.tostring(xml.getroot()))
TRAJ.append(traj)
self.data = TRAJ
return
def create_trajectory(turbine, joints, joints_vel, joints_acc, times):
""" This method creates the trajectory specification in OpenRave format and insert the waypoints:
joints - vels - accs - times
DOF - DOF - DOF - DOF - 1
Args:
turbine: (@ref Turbine) turbine object
joints: (float[n<SUB>i</SUB>][nDOF]) joints to create the trajectory
joints_vel: (float[n<SUB>i</SUB>][nDOF]) joints_vel to create the trajectory
joints_acc: (float[n<SUB>i</SUB>][nDOF]) joints_acc to create the trajectory
times: (float[n<SUB>i</SUB>]) deltatimes
Returns:
trajectory: OpenRave object.
"""
robot = turbine.robot
ind = str()
for i in range(robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + robot.GetName() + ' ' + ind,
len(robot.GetActiveDOFIndices()), 'cubic')
cs.AddDerivativeGroups(1, False)
cs.AddDerivativeGroups(2, False)
_ = cs.AddDeltaTimeGroup()
traj = RaveCreateTrajectory(turbine.env, '')
traj.Init(cs)
for i in range(len(joints)):
waypoint = list(joints[i])
waypoint.extend(list(joints_vel[i]))
waypoint.extend(list(joints_acc[i]))
waypoint.extend([times[i]])
traj.Insert(traj.GetNumWaypoints(), waypoint)
return traj
def test_serialize(self):
""" This test verifies if the class can serialize a path in openrave format.
"""
ind = str()
for i in range(self.robot.GetDOF()):
ind += str(i) + ' '
cs = ConfigurationSpecification()
_ = cs.AddGroup('joint_values ' + self.robot.GetName() + ' ' + ind,
len(self.robot.GetActiveDOFIndices()), 'cubic')
traj = RaveCreateTrajectory(self.turbine.env, '')
traj.Init(cs)
for i in range(len(self.joints)):
waypoint = list(self.joints[i])
traj.Insert(traj.GetNumWaypoints(), waypoint)
path = blade_coverage.Path(self.rays)
path.data = [traj]
try:
path.serialize('test_serialize')
shutil.rmtree('test_serialize')
except:
self.assertTrue(False, msg='Data can not be serialized')