예제 #1
0
class TestSampleRobot(unittest.TestCase):

    @classmethod
    def setUpClass(self):
        self.robot = HrpsysConfigurator()
        self.robot.init("SampleRobot(Robot)0", rospkg.RosPack().get_path("openhrp3")+"/share/OpenHRP-3.1/sample/model/sample1.wrl")


    def test_ik(self):
        self.robot.seq_svc.addJointGroup("LARM", ["LARM_SHOULDER_P", "LARM_SHOULDER_R", "LARM_SHOULDER_Y", "LARM_ELBOW", "LARM_WRIST_Y", "LARM_WRIST_P", "LARM_WRIST_R"])
        lav = [60,0,0,-90,0,0,0]
        for av in [[60, 0, 0, -80, 0, 0, 0],[30, 30, 0,-80, 0, 0, 0],
                   [40,40,20, -20, 0, 0, 0],[30, 30,20,-80,20,0, 20]]:
            self.robot.setJointAnglesOfGroup("LARM", av, 5)
            self.robot.waitInterpolationOfGroup("LARM")
            pos1 = self.robot.getReferencePosition("LARM_WRIST_R")
            rpy1 = self.robot.getReferenceRPY("LARM_WRIST_R")
            self.robot.setJointAnglesOfGroup("LARM", lav, 5)
            self.robot.waitInterpolationOfGroup("LARM")
            self.assertTrue(self.robot.setTargetPose("LARM", pos1, rpy1, 10))
            self.robot.waitInterpolationOfGroup("LARM")
            pos2 = self.robot.getReferencePosition("LARM_WRIST_R")
            rpy2 = self.robot.getReferenceRPY("LARM_WRIST_R")
            print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
            print "rpy", rpy1, rpy2, numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))
            self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<5.0e-3)
            self.assertTrue(numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))<5.0e-3)
            lav = av
예제 #2
0
class TestPA10(unittest.TestCase):

    @classmethod
    def setUpClass(self):
        self.robot = HrpsysConfigurator()
        self.robot.init("PA10Controller(Robot)0", rospkg.RosPack().get_path("openhrp3")+"/share/OpenHRP-3.1/sample/model/PA10/pa10.main.wrl")

    def test_ik(self):
        self.robot.seq_svc.addJointGroup("ARM", ["J1", "J2", "J3", "J4", "J5", "J6"])
        lav = [60,0,0,-90,0,0]
        for av in [[60, 0, 0, -80, 0, 0],[30, 30, 0,-80, 0, 0],
                   [40,40,20, -20, 0, 0],[30, 30,20,-80,20,20]]:
            self.robot.setJointAnglesOfGroup("ARM", av, 5)
            self.robot.waitInterpolationOfGroup("ARM")
            pos1 = self.robot.getReferencePosition("J6")
            rpy1 = self.robot.getReferenceRPY("J6")
            self.robot.setJointAnglesOfGroup("ARM", lav, 5)
            self.robot.waitInterpolationOfGroup("ARM")
            self.assertTrue(self.robot.setTargetPose("ARM", pos1, rpy1, 10))
            self.robot.waitInterpolationOfGroup("ARM")
            pos2 = self.robot.getReferencePosition("J6")
            rpy2 = self.robot.getReferenceRPY("J6")
            print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))
            print "rpy", rpy1, rpy2, numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))
            self.assertTrue(numpy.linalg.norm(numpy.array(pos1)-numpy.array(pos2))<5.0e-3)
            self.assertTrue(numpy.linalg.norm(numpy.array(rpy1)-numpy.array(rpy2))<5.0e-3)
            lav = av
예제 #3
0
class TestHRP4C(unittest.TestCase):
    @classmethod
    def setUpClass(self):
        self.robot = HrpsysConfigurator()
        self.robot.init(
            "HRP4C(Robot)0",
            rospkg.RosPack().get_path("hrpsys") +
            "/share/hrpsys/samples/HRP4C/HRP4Cmain.wrl")

    def test_walk(self):
        self.robot.loadPattern(
            rospkg.RosPack().get_path("hrpsys") +
            "/share/hrpsys/samples/HRP4C/data/walk2m", 1.0)
        self.robot.waitInterpolation()

        print self.robot.getReferencePosition("WAIST")
        print self.robot.getReferenceRotation("WAIST")
        self.assertTrue(
            numpy.linalg.norm(
                self.robot.getReferencePosition("WAIST") -
                numpy.array([0, 0, 0.78415])) < 1.0e-6)
        self.assertTrue(
            numpy.array_equal(self.robot.getReferenceRotation("WAIST"),
                              numpy.identity(3)))

    def test_ik(self):
        self.robot.seq_svc.addJointGroup("LARM", [
            "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P",
            "L_WRIST_Y", "L_WRIST_R"
        ])
        lav = [60, 0, 0, -90, 0, 0]
        for av in [[60, 0, 0, -80, 0, 0], [30, 30, 0, -80, 0, 0],
                   [40, 40, 20, -20, 0, 0], [30, 30, 20, -80, 20, 20]]:
            self.robot.setJointAnglesOfGroup("LARM", av, 5)
            self.robot.waitInterpolationOfGroup("LARM")
            pos1 = self.robot.getReferencePosition("L_WRIST_R")
            rpy1 = self.robot.getReferenceRPY("L_WRIST_R")
            self.robot.setJointAnglesOfGroup("LARM", lav, 5)
            self.robot.waitInterpolationOfGroup("LARM")
            self.assertTrue(self.robot.setTargetPose("LARM", pos1, rpy1, 10))
            self.robot.waitInterpolationOfGroup("LARM")
            pos2 = self.robot.getReferencePosition("L_WRIST_R")
            rpy2 = self.robot.getReferenceRPY("L_WRIST_R")
            print "pos", pos1, pos2, numpy.linalg.norm(
                numpy.array(pos1) - numpy.array(pos2))
            print "rpy", rpy1, rpy2, numpy.linalg.norm(
                numpy.array(rpy1) - numpy.array(rpy2))
            self.assertTrue(
                numpy.linalg.norm(numpy.array(pos1) -
                                  numpy.array(pos2)) < 5.0e-3)
            self.assertTrue(
                numpy.linalg.norm(numpy.array(rpy1) -
                                  numpy.array(rpy2)) < 5.0e-3)
            lav = av
예제 #4
0
class TestHRP4C(unittest.TestCase):
    @classmethod
    def setUpClass(self):
        self.robot = HrpsysConfigurator()
        self.robot.init(
            "HRP4C(Robot)0", rospkg.RosPack().get_path("hrpsys") + "/share/hrpsys/samples/HRP4C/HRP4Cmain.wrl"
        )

    def test_walk(self):
        self.robot.loadPattern(rospkg.RosPack().get_path("hrpsys") + "/share/hrpsys/samples/HRP4C/data/walk2m", 1.0)
        self.robot.waitInterpolation()

        print self.robot.getReferencePosition("WAIST")
        print self.robot.getReferenceRotation("WAIST")
        self.assertTrue(
            numpy.linalg.norm(self.robot.getReferencePosition("WAIST") - numpy.array([0, 0, 0.78415])) < 1.0e-6
        )
        self.assertTrue(numpy.array_equal(self.robot.getReferenceRotation("WAIST"), numpy.identity(3)))

    def test_ik(self):
        self.robot.seq_svc.addJointGroup(
            "LARM", ["L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P", "L_WRIST_Y", "L_WRIST_R"]
        )
        lav = [60, 0, 0, -90, 0, 0]
        for av in [[60, 0, 0, -80, 0, 0], [30, 30, 0, -80, 0, 0], [40, 40, 20, -20, 0, 0], [30, 30, 20, -80, 20, 20]]:
            self.robot.setJointAnglesOfGroup("LARM", av, 5)
            self.robot.waitInterpolationOfGroup("LARM")
            pos1 = self.robot.getReferencePosition("L_WRIST_R")
            rpy1 = self.robot.getReferenceRPY("L_WRIST_R")
            self.robot.setJointAnglesOfGroup("LARM", lav, 5)
            self.robot.waitInterpolationOfGroup("LARM")
            self.assertTrue(self.robot.setTargetPose("LARM", pos1, rpy1, 10))
            self.robot.waitInterpolationOfGroup("LARM")
            pos2 = self.robot.getReferencePosition("L_WRIST_R")
            rpy2 = self.robot.getReferenceRPY("L_WRIST_R")
            print "pos", pos1, pos2, numpy.linalg.norm(numpy.array(pos1) - numpy.array(pos2))
            print "rpy", rpy1, rpy2, numpy.linalg.norm(numpy.array(rpy1) - numpy.array(rpy2))
            self.assertTrue(numpy.linalg.norm(numpy.array(pos1) - numpy.array(pos2)) < 5.0e-3)
            self.assertTrue(numpy.linalg.norm(numpy.array(rpy1) - numpy.array(rpy2)) < 5.0e-3)
            lav = av
예제 #5
0
class TestPA10(unittest.TestCase):
    @classmethod
    def setUpClass(self):
        self.robot = HrpsysConfigurator()
        self.robot.init(
            "PA10Controller(Robot)0",
            rospkg.RosPack().get_path("openhrp3") +
            "/share/OpenHRP-3.1/sample/model/PA10/pa10.main.wrl")

    def test_ik(self):
        self.robot.seq_svc.addJointGroup("ARM",
                                         ["J1", "J2", "J3", "J4", "J5", "J6"])
        lav = [60, 0, 0, -90, 0, 0]
        for av in [[60, 0, 0, -80, 0, 0], [30, 30, 0, -80, 0, 0],
                   [40, 40, 20, -20, 0, 0], [30, 30, 20, -80, 20, 20]]:
            self.robot.setJointAnglesOfGroup("ARM", av, 5)
            self.robot.waitInterpolationOfGroup("ARM")
            pos1 = self.robot.getReferencePosition("J6")
            rpy1 = self.robot.getReferenceRPY("J6")
            self.robot.setJointAnglesOfGroup("ARM", lav, 5)
            self.robot.waitInterpolationOfGroup("ARM")
            self.assertTrue(self.robot.setTargetPose("ARM", pos1, rpy1, 10))
            self.robot.waitInterpolationOfGroup("ARM")
            pos2 = self.robot.getReferencePosition("J6")
            rpy2 = self.robot.getReferenceRPY("J6")
            print "pos", pos1, pos2, numpy.linalg.norm(
                numpy.array(pos1) - numpy.array(pos2))
            print "rpy", rpy1, rpy2, numpy.linalg.norm(
                numpy.array(rpy1) - numpy.array(rpy2))
            self.assertTrue(
                numpy.linalg.norm(numpy.array(pos1) -
                                  numpy.array(pos2)) < 5.0e-3)
            self.assertTrue(
                numpy.linalg.norm(numpy.array(rpy1) -
                                  numpy.array(rpy2)) < 5.0e-3)
            lav = av