Python vectorToTuple Examples

Example #1

File: dynpg.py Project: elie-moussy/sot-dyninv

xmlDir    = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value    = gearRatio[robotName]

plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')

Example #2

jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit - llimit
mlimit = (ulimit + llimit) / 2
llimit[6:12] = mlimit[6:12] - dlimit[6:12] * 0.49
ulimit[0, 10] = mlimit[0, 10] + dlimit[0, 10] * 0.45
ulimit[0, 7] = mlimit[0, 7] + dlimit[0, 7] * 0.45
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state, dyn.position)
plug(robot.velocity, dyn.velocity)
dyn.acceleration.value = robotDim * (0., )

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics', 'true')
dyn.setProperty('ComputeAccelerationCoM', 'true')

Example #3

xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit - llimit
mlimit = (ulimit + llimit) / 2
llimit[6:18] = mlimit[6:12] - dlimit[6:12] * 0.48
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state, dyn.position)
plug(robot.velocity, dyn.velocity)
dyn.acceleration.value = robotDim * (0., )

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics', 'true')
dyn.setProperty('ComputeAccelerationCoM', 'true')

Example #4

File: imit.py Project: stack-of-tasks/sot-dyninv

specificitiesPath = xmlDir + "/HRP2SpecificitiesSmall.xml"
jointRankPath = xmlDir + "/HRP2LinkJointRankSmall.xml"

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit - llimit
mlimit = (ulimit + llimit) / 2
llimit[6:12] = mlimit[6:12] - dlimit[6:12] * 0.49
ulimit[0, 10] = mlimit[0, 10] + dlimit[0, 10] * 0.49
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state, dyn.position)
plug(robot.velocity, dyn.velocity)
dyn.acceleration.value = robotDim * (0.0,)

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty("ComputeBackwardDynamics", "true")
dyn.setProperty("ComputeAccelerationCoM", "true")

Example #5

File: imit.py Project: stack-of-tasks/sot-dyninv

def gotoq(qref, selec=None):
    featurePostureDes.errorIN.value = vectorToTuple(qref.transpose())
    if selec != None:
        featurePosture.selec.value = toFlags(selec)

Example #6

File: mocap_parser.py Project: nim65s/sot-dyninv

 def jointAngles(self, t=None):
     if t == None: t = self.timeDecimal
     q1 = matrix(MocapParser.jointAngles(self, int(floor(t))))
     q2 = matrix(MocapParser.jointAngles(self, int(ceil(t))))
     dt = self.timeDecimal % 1
     return vectorToTuple((1 - dt) * q1 + dt * q2)

Example #7

File: mocap_parser.py Project: elie-moussy/sot-dyninv

 def jointAngles(self,t=None):
     if t==None: t=self.timeDecimal
     q1=matrix(MocapParser.jointAngles(self,int(floor(t))))
     q2=matrix(MocapParser.jointAngles(self,int(ceil(t))))
     dt=self.timeDecimal%1
     return vectorToTuple( (1-dt)*q1+dt*q2 ) 

Example #8

def gotoq( qref,selec=None ):
    featurePostureDes.errorIN.value = vectorToTuple( qref.transpose() )
    if selec!=None: featurePosture.selec.value = toFlags( selec )

Example #9

xmlDir    = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value    = gearRatio[robotName]

plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')

Example #10

xmlDir    = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value    = gearRatio[robotName]

plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')