Exemplo n.º 1
0
    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')
Exemplo n.º 2
0
    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
Exemplo n.º 3
0
    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
Exemplo n.º 4
0
    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')