Exemplo n.º 1
0
    def handSymmetry(self, vector):
        """
        Conversion of local coordinates from left hand to right hand

          Input:
            - a vector: locally expressed in the local frame of left wrist,
          Return:
            - a vector: locally expressed in the local frame of the right wrist.

          The conversion is done in such a way that input and output are
          symmetric with respect to plane (xz) in global frame.
        """
        # input vector expressed in global frame
        vector = R3(vector)
        globalLeftVector = SE3(self.leftWristPosition) * vector
        globalRightVector = R3(globalLeftVector)
        globalRightVector = R3(globalRightVector[0], -globalRightVector[1],
                               globalRightVector[2])
        output = SE3(self.rightWristPosition).inverse() * globalRightVector
        return tuple(output)
Exemplo n.º 2
0
 def setReferenceValue (self, gui, rosexport, refs):
     from dynamic_graph.sot.tools.quaternion import Quaternion
     from dynamic_graph.sot.tools.se3 import SE3
     idx = 0
     for n, types in refs.items():
         for i,type in idx_zip(types):
             nameref = "ref_" + n + str(i)
             sig = rosexport.signal(n)
             # if sig.time != self.sotrobot.device.control.time or len(sig.value) == 0:
             if len(sig.value) == 0:
                 if gui.getIntProperty(nameref, "visibility") != 2:
                     print "Hiding", nameref
                     gui.setVisibility(nameref, 'OFF')
                 continue
             else:
                 if gui.getIntProperty(nameref, "visibility") == 2:
                     print "Showing", nameref
                     # gui.setVisibility(nameref, 'ON')
                     gui.setVisibility(nameref, 'ALWAYS_ON_TOP')
             if type == 'pose':
                 H = SE3(sig.value)
                 q = Quaternion(sig.value)
                 gui.applyConfiguration(nameref, list(H.translation.value) + list(q.array[1:]) + list(q.array[:1]) )
     gui.refresh()
Exemplo n.º 3
0
def convert(filename):
    """
    Convert a seqplay file in OpenHRP format to sot-tool format
    """
    global pos, zmp, hip
    openhrpPos = np.genfromtxt(filename + '.pos')
    openhrpZmp = np.genfromtxt(filename + '.zmp')
    nbConfig = len(openhrpPos)
    if len(openhrpZmp) != nbConfig:
        raise RuntimeError(filename + ".pos and " + filename + ".zmp have different lengths.")
    try:
        openhrpHip = np.genfromtxt(filename + '.hip')
    except IOError:
        hip = []
        for i in range(len(openhrpPos)):
            hip.append((
                openhrpPos[i][0],
                0,
                0,
                0,
            ))
        openhrpHip = np.array(hip)

    if len(openhrpHip) != nbConfig:
        raise RuntimeError(filename + ".pos and " + filename + ".hip have different lengths.")

    t = 1
    featurePos = []
    featureLa = []
    featureRa = []
    featureCom = []
    forceRightFoot = []
    forceLeftFoot = []

    fixedFoot = None
    fixedLeftFoot = None
    fixedRightFoot = None
    for (pos, zmp, hip) in zip(openhrpPos, openhrpZmp, openhrpHip):
        translation = 3 * (0., )
        config = list(translation + tuple(hip[1:]) + tuple(pos[1:31]))
        robot.dynamic.position.value = tuple(config)
        robot.dynamic.position.time = t
        robot.com.recompute(t)
        robot.leftAnkle.position.recompute(t)
        robot.rightAnkle.position.recompute(t)
        lf = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
        rf = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)
        # find support foot
        rpy2matrix.sin.value = tuple(hip[1:])
        rpy2matrix.sout.recompute(t)
        waist = SE3(rpy2matrix.sout.value, translation)
        zmpGlob = waist * R3(tuple(zmp[1:]))
        # fr = m g * (zmpGlob - lf | rf - lf)/||rf - lf||^2
        # fl = (m g - fr)
        fr = m * g * ((zmpGlob - lf) * (rf - lf) / ((rf - lf) * (rf - lf)))
        fl = m * g - fr
        if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob):
            supportFoot = lf
            fixedFoot = fixedLeftFoot
        else:
            supportFoot = rf
            fixedFoot = fixedRightFoot
        t += 1
        # move support foot to previous value
        if fixedFoot is None:
            config[2] -= supportFoot[2]
        else:
            config[0] += fixedFoot[0] - supportFoot[0]
            config[1] += fixedFoot[1] - supportFoot[1]
            config[2] += fixedFoot[2] - supportFoot[2]

        robot.dynamic.position.value = tuple(config)
        robot.dynamic.position.time = t
        robot.com.recompute(t)
        robot.leftAnkle.position.recompute(t)
        robot.rightAnkle.position.recompute(t)
        featurePos.append(config)
        featureCom.append(robot.com.value)
        featureLa.append(robot.leftAnkle.position.value)
        featureRa.append(robot.rightAnkle.position.value)
        forceLeftFoot.append((
            0.,
            0.,
            fl,
            0.,
            0.,
            0.,
        ))
        forceRightFoot.append((
            0.,
            0.,
            fr,
            0.,
            0.,
            0.,
        ))
        t += 1
        fixedLeftFoot = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
        fixedRightFoot = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)

    filePos = open(filename + '.posture', 'w')
    fileLa = open(filename + '.la', 'w')
    fileRa = open(filename + '.ra', 'w')
    fileCom = open(filename + '.com', 'w')
    fileFl = open(filename + '.fl', 'w')
    fileFr = open(filename + '.fr', 'w')

    dt = .005
    for (pos, la, ra, com, force_lf, force_rf, i) in zip(featurePos, featureLa, featureRa, featureCom, forceLeftFoot,
                                                         forceRightFoot, range(10000000)):
        t = i * dt
        filePos.write("{0}".format(t))
        fileLa.write("{0}".format(t))
        fileRa.write("{0}".format(t))
        fileCom.write("{0}".format(t))
        fileFl.write("{0}".format(t))
        fileFr.write("{0}".format(t))

        for x in pos:
            filePos.write("\t{0}".format(x))

        for row in la:
            for x in row:
                fileLa.write("\t{0}".format(x))

        for row in ra:
            for x in row:
                fileRa.write("\t{0}".format(x))

        for x in com:
            fileCom.write("\t{0}".format(x))

        for x in force_lf:
            fileFl.write("\t{0}".format(x))

        for x in force_rf:
            fileFr.write("\t{0}".format(x))

        filePos.write("\n")
        fileLa.write("\n")
        fileRa.write("\n")
        fileCom.write("\n")
        fileFl.write("\n")
        fileFr.write("\n")

    filePos.close()
    fileLa.close()
    fileRa.close()
    fileCom.close()
    fileFl.close()
    fileFr.close()