def drawArrow(self, startPos, endPos, vector=None, lineWidth=.02):
if vector == None:
vector = [endPos[i] - startPos[i] for i in range(3)]
elif startPos == None:
startPos = [endPos[i] - vector[i] for i in range(3)]
length = mm.length(vector)
if length == 0.: return
glPushMatrix()
arrowT = mm.Rp2T(mm.getSO3FromVectors((length, 0, 0), vector),
startPos)
glMultMatrixf(arrowT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
# line + cone all parts
glePolyCone(
((0, 0, 0), (0, 0, 0), (length - triLength, 0, 0),
(length - triLength, 0, 0), (length, 0, 0), (length, 0, 0)), None,
(lineWidth / 2., lineWidth / 2., lineWidth / 2., triWidth / 2., 0,
0))
glPopMatrix()
def getSpringLengthsFromMesh(mesh, springConfigs):
springLengths = [None] * len(springConfigs)
for i in range(len(springConfigs)):
springLengths[i] = mm.length(
mesh.getVertexPosition(springConfigs[i].particleIndex0) -
mesh.getVertexPosition(springConfigs[i].particleIndex1))
return springLengths
def solve(self, inoutPosture, targetPositions):
iterNum = 0
positionDiff = 0.
while True:
currentPositions = [inoutPosture.getPosition(self.effectorIndices[0])]
axes = self.calcJacobian(inoutPosture, currentPositions, targetPositions)
forceVec = targetPositions[0] - currentPositions[0]
# dt = J * dtheta
# Jt * dt = Jt * J * dtheta
# (Jt * J)^-1 * Jt * dt = dtheta
#deltaThetas = np.dot(np.dot(np.linalg.inv(np.dot(np.transpose(self.jacobian),self.jacobian)),np.transpose(self.jacobian)), forceVec)*.1
# moore-penrose pseudo inverse
deltaThetas = np.dot(np.linalg.pinv(self.jacobian), forceVec)*.1
self.applyThetas(deltaThetas, axes, inoutPosture)
iterNum += 1
print(iterNum)
if iterNum > self.iterationLimit:
print('iter')
break;
positionDiff = 0.
for i in range(len(targetPositions)):
positionDiff += mm.length(targetPositions[i] - inoutPosture.getPosition( self.effectorIndices[i]) )
if positionDiff < self.positionThreshold:
print('converge', iterNum)
break;
def distanceByRelPos2(jointPosture0, jointPosture1):
distance = 0.
rootPos0 = jointPosture0.rootPos
rootPos1 = jointPosture1.rootPos
for i in range(jointPosture0.skeleton.getJointNum()):
relPos0 = jointPosture0.getJointPositionGlobal(i) - rootPos0
relPos1 = jointPosture1.getJointPositionGlobal(i) - rootPos1
distance += mmMath.length(relPos0 - relPos1)
return distance
def distanceByRelPos(jointPosture0, jointPosture1):
distance = 0.
rootPos0 = jointPosture0.getPosition(jointPosture0.skeleton.rootIndex)
rootPos1 = jointPosture1.getPosition(jointPosture1.skeleton.rootIndex)
for i in range(jointPosture0.skeleton.getElementNum()):
relPos0 = jointPosture0.getPosition(i) - rootPos0
relPos1 = jointPosture1.getPosition(i) - rootPos1
distance += mmMath.length(relPos0 - relPos1)
return distance
def distanceByRelPos2(jointPosture0, jointPosture1):
distance = 0.
rootPos0 = jointPosture0.rootPos
rootPos1 = jointPosture1.rootPos
for i in range(jointPosture0.skeleton.getJointNum()):
relPos0 = jointPosture0.getJointPositionGlobal(i) - rootPos0
relPos1 = jointPosture1.getJointPositionGlobal(i) - rootPos1
distance += mmMath.length(relPos0 - relPos1)
return distance
def calcPDTorque(Rpd, Rcd, Rpc, Rcc, Wpc, Wcc, Kp, Kd, joint):
Rpc = mm.odeSO3ToSO3(Rpc)
Rcc = mm.odeSO3ToSO3(Rcc)
Ra = numpy.dot(Rpc, Rpd.transpose())
Rcd = numpy.dot(Ra, Rcd)
dR = mm.logSO3(numpy.dot(Rcd, Rcc.transpose()))
dW = numpy.array(
[-Wpc[0] + Wcc[0], -Wpc[1] + Wcc[1], -Wpc[2] + Wcc[2]])
# joint.setAxis(0,0,dR)
# joint.setAxis(1,0,dW)
joint.setAxis(0, 0, dR - dW)
# joint.addTorques(-Kp*mm.length(dR),0,0)
# joint.addTorques(0,Kd*mm.length(dW),0)
joint.addTorques(-Kp * mm.length(dR) - Kd * mm.length(dW), 0, 0)
def distanceByRelPos(jointPosture0, jointPosture1):
distance = 0.
rootPos0 = jointPosture0.getPosition(jointPosture0.skeleton.rootIndex)
rootPos1 = jointPosture1.getPosition(jointPosture1.skeleton.rootIndex)
for i in range(jointPosture0.skeleton.getElementNum()):
relPos0 = jointPosture0.getPosition(i) - rootPos0
relPos1 = jointPosture1.getPosition(i) - rootPos1
distance += mmMath.length(relPos0 - relPos1)
return distance
def mergePoints(mesh, distance = 0.0):
len_vertices = len(mesh.vertices)
mergeMap = {}
for fromIndex in range(len_vertices):
for toIndex in range(fromIndex+1, len_vertices):
length = mmMath.length(mesh.vertices[fromIndex].pos - mesh.vertices[toIndex].pos)
if length <= distance:
mergeMap[fromIndex] = toIndex
# print mergeMap
transitions = [i for i in range(len_vertices)]
# print transitions
for i in range(len_vertices-1,-1,-1):
# print i
if i in mergeMap:
toBeMerged = i
# print 'toBeMerged', toBeMerged
for j in range(toBeMerged+1, len_vertices):
transitions[j] -= 1
for key, targetIndex in mergeMap.items():
if targetIndex >= toBeMerged:
# print key, ':', mergeMap[key], '->', mergeMap[key]-1
mergeMap[key] -= 1
# print transitions
# print mergeMap
for toBeMerged in mergeMap:
transitions[toBeMerged] = mergeMap[toBeMerged]
# print transitions
# reorder vertex list
old_vertices = mesh.vertices
mesh.vertices = [None]*(len_vertices - len(mergeMap))
for i in range(len(old_vertices)):
mesh.vertices[transitions[i]] = old_vertices[i]
# update face
for f in mesh.faces:
for j in range(len(f.vertexIndex)):
f.vertexIndex[j] = transitions[f.vertexIndex[j]]
for f in mesh.faces[:]:
for i in range(len(f.vertexIndex)):
for j in range(i+1, len(f.vertexIndex)):
if f.vertexIndex[i] == f.vertexIndex[j]:
mesh.faces.remove(f)
if(isinstance(mesh, yms.SkinMesh)):
# reorder bone weight list
old_vertexBoneWeights = mesh.vertexBoneWeights
mesh.vertexBoneWeights = [None]*(len_vertices - len(mergeMap))
for i in range(len(old_vertexBoneWeights)):
mesh.vertexBoneWeights[transitions[i]] = old_vertexBoneWeights[i]
mesh.initialize(mesh.initialPosture)
def getElementContactStates(motion, element_name_or_index, hRef, vRef):
if isinstance(element_name_or_index, int): index = element_name_or_index
else: index = motion[0].skeleton.getElementIndex(element_name_or_index)
contactStates = [None] * len(motion)
for i in range(len(motion)):
pos = motion.getPosition(index, i)
vel = motion.getVelocity(index, i)
if pos[1] < hRef and mmMath.length(vel) < vRef:
contactStates[i] = True
else:
contactStates[i] = False
return contactStates
def getElementContactStates(motion, element_name_or_index, hRef, vRef):
if isinstance(element_name_or_index, int): index = element_name_or_index
else: index = motion[0].skeleton.getElementIndex(element_name_or_index)
contactStates = [None]*len(motion)
for i in range(len(motion)):
pos = motion.getPosition(index, i)
vel = motion.getVelocity(index, i)
if pos[1] < hRef and mmMath.length(vel) < vRef:
contactStates[i] = True
else:
contactStates[i] = False
return contactStates
def drawCircularArrow(self,
startPos,
endPos,
rotVec=None,
lineWidth=.02,
radius=.1):
if rotVec is None:
rotVec = [endPos[i] - startPos[i] for i in range(3)]
elif startPos is None:
startPos = [endPos[i] - rotVec[i] for i in range(3)]
length = mm.length(rotVec)
if length == 0.: return
glPushMatrix()
axisT = mm.r_p_to_t(mm.getSO3FromVectors((0, 0, length), rotVec),
startPos)
glMultMatrixf(axisT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
# axis
# self.drawLine((0,0,0), (0,0,length))
glePolyCylinder(((0, 0, 0), (0, 0, 0), (0, 0, length), (0, 0, length)),
None, lineWidth / 4.)
# circular line part
# gleHelicoid( rToroid , startRadius , drdTheta , startZ , dzdTheta ,
# startXform , dXformdTheta , startTheta , sweepTheta )
sweepTheta = 2 * math.pi * length * mm.DEG
gleHelicoid(lineWidth / 2., radius, 0., 0., radius, None, None, 0.,
sweepTheta)
# cone part
glPushMatrix()
glRotatef(sweepTheta, 0, 0, 1)
glTranslatef(radius, 0, radius * (sweepTheta / 360.))
glRotatef(-90, 1, 0, 0)
glePolyCone(
((0, 0, 0), (0, 0, 0), (0, 0, triLength), (0, 0, triLength)), None,
(triWidth / 2., triWidth / 2., 0, 0))
glPopMatrix()
glPopMatrix()
def draw2DArrow(self, startPos, endPos, vector=None, lineWidth=.02):
if vector == None:
vector = [endPos[i] - startPos[i] for i in range(3)]
elif startPos == None:
startPos = [endPos[i] - vector[i] for i in range(3)]
# glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
glDisable(GL_CULL_FACE)
glPushMatrix()
length = mm.length(vector)
arrowT = mm.Rp2T(mm.getSO3FromVectors((length, 0, 0), vector),
startPos)
glMultMatrixf(arrowT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
angles = [0, 90]
for angle in angles:
glRotatef(angle, 1, 0, 0)
# line part
glBegin(GL_QUADS)
glVertex3f(0, 0, lineWidth / 2)
glVertex3f(0, 0, -lineWidth / 2)
glVertex3f(length - triLength, 0, -lineWidth / 2)
glVertex3f(length - triLength, 0, +lineWidth / 2)
glEnd()
# triangle part
glBegin(GL_TRIANGLES)
glVertex3f(length - triLength, 0, triWidth / 2)
glVertex3f(length - triLength, 0, -triWidth / 2)
glVertex3f(length, 0, 0)
glEnd()
glPopMatrix()
glEnable(GL_CULL_FACE)
def solve(self, inoutPosture, targetPositions):
iterNum = 0
positionDiff = 0.
while True:
currentPositions = [inoutPosture.getPosition(self.effectorIndices[0])]
axes = self.calcJacobian(inoutPosture, currentPositions, targetPositions)
forceVec = targetPositions[0] - currentPositions[0]
deltaThetas = np.dot(np.transpose(self.jacobian), forceVec)*.1
self.applyThetas(deltaThetas, axes, inoutPosture)
iterNum += 1
if iterNum > self.iterationLimit:
print 'iter'
break;
positionDiff = 0.
for i in range(len(targetPositions)):
positionDiff += mm.length(targetPositions[i] - inoutPosture.getPosition( self.effectorIndices[i]) )
if positionDiff < self.positionThreshold:
print 'converge'
break;
def simulateCallback(frame):
print "main:frame : ", frame
curTime = time.time()
if frame % 30 == 1: pt[0] = time.time()
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
global desCOMOffset
motionModel.update(motion[0])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
############################
#Reference motion modulation
dCM_k = 10.
linkVelocities = controlModel.getBodyVelocitiesGlobal()
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.08 * np.sin(2 * 3.14 * leftHipTimer / 60.))
#viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.))
#B_Hipd = viewer.objectInfoWnd.labelLeftHip.value()
#newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.)
#idx = motion[0].skeleton.getJointIndex('LeftUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
#idx = motion[0].skeleton.getJointIndex('RightUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
leftHipTimer -= 1
timeReport[0] += time.time() - curTime
curTime = time.time()
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
linkInertias_ref = motionModel.getBodyInertiasGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
timeReport[1] += time.time() - curTime
curTime = time.time()
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
timeReport[2] += time.time() - curTime
curTime = time.time()
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if frame > 0:
dJsys = (Jsys - JsysPre) * 30.
else:
dJsys = (Jsys - Jsys)
JsysPre = Jsys.copy()
timeReport[3] += time.time() - curTime
curTime = time.time()
lcpBodyIDs, lcpContactPositions, lcpContactPositionLocals, lcpContactForces = hls.calcLCPForces(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., 4, None)
# bodyIDs : IDs for Virtual Physics, not VpModel !!!
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if (CP is not None):
CP[1] = 0.
for i in range(controlModel.getBodyNum()):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
for j in range(0, footPartNum):
jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy()
jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy()
dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6] #.copy()
dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6] #.copy()
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = .5 + desCOMOffset
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r +
controlModel.getBodyPositionGlobal(indexFootL[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r +
controlModel.getBodyPositionGlobal(indexFootR[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter = (
getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') +
getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4.
#footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(), 0,
viewer.objectInfoWnd.comOffsetZ.value()
])
#footCenter += footCenterOffset
vecRatio = mm.length(curRelCMVec) * 0.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
######################
# optimization terms
######################
# linear momentum
CM_ref_plane = footCenter + footCenterOffset
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] -
CM[1]) - Dl * totalMass * dCM[1]
#dL_des_plane[1] = 0.
#print 'dL_des_plane', dL_des_plane
# angular momentum
CP_ref = footCenter + footCenterOffset
CP_ref[1] = 0.
timeStep = 30.
if (CP_old[0] is None) or (CP is None):
dCP = None
else:
dCP = (CP - CP_old[0]) * timeStep
CP_old[0] = CP
if (CP is not None) and (dCP is not None):
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#print 'dCP: ', dCP
#print 'ddCP_des: ', ddCP_des
#print 'CP_des: ', CP_des
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross(
(CP_des - CM_plane),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
flagContact = True
if (dH_des is None) or np.any(np.isnan(dH_des)) == True:
flagContact = False
#viewer.doc.showRenderer('rd_grf_des', False)
#viewer.motionViewWnd.update(1, viewer.doc)
#else:
#viewer.doc.showRenderer('rd_grf_des', True)
#viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
#MOTION = FORWARD_JUMP
if mit.MOTION == mit.FORWARD_JUMP:
frame_index = [136, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO2:
frame_index = [130 + 40, 100]
elif mit.MOTION == mit.WALK:
frame_index = [10000, 60]
elif mit.MOTION == mit.TIPTOE:
frame_index = [1000000, 1000000]
#frame_index = [10000, 165]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
if frame > frame_index[0]:
if stage != POWERFUL_BALANCING:
print("#", frame, "-POWERFUL_BALANCING")
stage = POWERFUL_BALANCING
Kk = Kk * 2
Dk = 2 * (Kk**.5)
elif frame > frame_index[1]:
if stage != MOTION_TRACKING:
print("#", frame, "-MOTION_TRACKING")
stage = MOTION_TRACKING
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if 0: #stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt * 2
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
if flagContact == True:
if stage != MOTION_TRACKING + 10:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R,
r_bias)
#mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
# using || dH ||^2 instead
mot.addAnotherTerms(problem, totalDOF, Bh, S,
-(s_bias + Kh * np.dot(S, dth_flat)))
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
# Hard constraint
Kk2 = Kk * 4.0
Dk2 = 2 * (Kk2**.5)
ankleW = 0
ankleOffset = ankleW * curRelCMVec[2]
metatarW = 0
metatarOffset = metatarW * curRelCMVec[2]
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING and frame > 5:
# ankle strategy
idx = 0 #LEFT/RIGHT_TOES
if mit.FOOT_PART_NUM == 1:
yOffset = 0.03
else:
yOffset = 0.069
#yOffset = 0.06
# ankleOffset = (footCenter - CM_plane)*4.
ankleOffset = footCenterOffset * 10.
ankleOffset[1] = 0.
#ankleOffset[2] = 0.
ankleOffset[2] = ankleOffset[2] * 20.
ankleOffsetL = ankleOffset.copy()
ankleOffsetR = ankleOffset.copy()
#ankleOffset= np.array((0,0,0))
if footCenterOffset[0] > 0.0:
ankleOffsetL[0] = 0.
else:
ankleOffsetR[0] = 0.
# print 'ankleOffset=', ankleOffset
desLinearAccL, desPosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[idx], ModelOffset,
CM_ref, CM, Kk, Dk, yOffset) #0.076) #0.14)
desLinearAccR, desPosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[idx], ModelOffset,
CM_ref, CM, Kk, Dk, yOffset)
ax = [0, 0, -1]
aaa = getBodyGlobalOri(controlModel, motion, 'RightFoot')
#print np.dot(aaa, ax)
if mit.FOOT_PART_NUM == 1:
ax = [0, 1, 0]
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[idx], Kk, Dk, ax,
mm.normalize([0, 1, 0] + ankleOffsetL))
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[idx], Kk, Dk, ax,
mm.normalize([0, 1, 0] + ankleOffsetR))
a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)),
np.hstack((desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((jFootL[idx], jFootR[idx]))
dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx]))
#mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.addConstraint(problem, totalDOF, Jsup_2[:1], dJsup_2[:1], dth_flat, a_sup_2[:1])
#mot.addConstraint(problem, totalDOF, Jsup_2[2:], dJsup_2[2:], dth_flat, a_sup_2[2:])
#mot.addConstraint(problem, totalDOF, Jsup_2[3:], dJsup_2[3:], dth_flat, a_sup_2[3:])
mot.addAnotherTerms(problem, totalDOF,
viewer.objectInfoWnd.Bc.value(), Jsup_2[3:],
a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[0:1], a_sup_2[0:1] - np.dot(dJsup_2[0:1] , dth_flat))
#mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[2:], a_sup_2[2:] - np.dot(dJsup_2[2:] , dth_flat))
desCOMOffset = 0.0
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
if stage == STATIC_BALANCING and frame > 10: # and False:
del rd_desPoints[:]
# foot strategy
#Kk2 = Kk * 2.5
#Kk2 = Kk * .2
#Dk2 = 2*(Kk2**.5)
desForePosL = [0, 0, 0]
desForePosR = [0, 0, 0]
desRearPosL = [0, 0, 0]
desRearPosR = [0, 0, 0]
footPartPos = []
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('LeftCalcaneus_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('LeftPhalange_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightCalcaneus_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightPhalange_1')))
for i in range(1, footPartNum):
contactFlagFootL[i] = 1
contactFlagFootR[i] = 1
SupPts = np.vstack(
(np.array((footPartPos[0][0], footPartPos[1][0],
footPartPos[2][0], footPartPos[3][0])),
np.array(
(footPartPos[0][2], footPartPos[1][2], footPartPos[2][2],
footPartPos[3][2])), np.array((1., 1., 1., 1.))))
coordWidthLen = 2.
coordLengthLen = 1.5
SupUV = np.vstack(
(np.array((-coordWidthLen, -coordWidthLen, coordWidthLen,
coordWidthLen)),
np.array((-coordLengthLen, coordLengthLen, -coordLengthLen,
coordLengthLen)), np.array((1., 1., 1., 1.))))
SupMap = np.dot(np.dot(SupUV, SupUV.T),
np.linalg.inv(np.dot(SupPts, SupUV.T)))
#print SupMap
desFootCenter = footCenter + footCenterOffset
footCenterPts = np.array((desFootCenter[0], desFootCenter[2], 1))
#print np.dot(SupMap, footCenterPts)
#print np.dot(getBodyGlobalOri(controlModel, motion, 'LeftMetatarsal_1'), np.array((0,1,0)))
CM_plane_2D = np.array((CM[0], CM[2], 1))
# CM_plane_UV = np.dot(SupMap, CM_plane_2D)
CM_plane_UV = np.dot(SupMap, footCenterPts)
# print CM_plane_UV
# for i in range(1, footPartNum):
if CM_plane_UV[1] > .5:
# com is in front
for i in range(1, 5):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
elif CM_plane_UV[1] < -.5:
# com is back
for i in range(3, footPartNum):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
else:
# com is in middle position
for i in range(3, 5):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
contactFlagFoot = contactFlagFootL
if CM_plane_UV[0] < 0.:
contactFlagFoot = contactFlagFootR
# CM_plane_UV[0] = -CM_plane_UV[0]
if abs(CM_plane_UV[0]) > 1.:
for j in range(0, 3):
contactFlagFoot[2 * j + 2] = 0
# print 'footL : ',contactFlagFootL
# print 'footR : ',contactFlagFootR
for i in range(1, footPartNum):
axis = [0, 0, 1]
if i == 1 or i == 2:
axis = [0, 0, -1]
desAng = [0, 0, 1]
if i == 1 or i == 2:
desAng = [0, 0, -1]
desY = 0.029
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[i], ModelOffset,
CM_ref, CM, Kk2, Dk2, desY)
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2,
axis, desAng)
a_sup_2 = np.hstack((desLinearAccL, desAngularAccL))
Jsup_2 = jFootL[i].copy()
dJsup_2 = dJFootL[i].copy()
mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2,
dth_flat, a_sup_2)
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:] , dth_flat))
rd_desPoints.append(desForePosL.copy())
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[i], ModelOffset,
CM_ref, CM, Kk2, Dk2, desY)
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2,
axis, desAng)
a_sup_2 = np.hstack((desLinearAccR, desAngularAccR))
Jsup_2 = jFootR[i].copy()
dJsup_2 = dJFootR[i].copy()
mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2,
dth_flat, a_sup_2)
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat))
rd_desPoints.append(desForePosR.copy())
rd_DesForePosL[0] = desForePosL
rd_DesForePosR[0] = desForePosR
rd_DesRearPosL[0] = desRearPosL
rd_DesRearPosR[0] = desRearPosR
##############################
#if Jsup_2 is not None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
r = problem.solve()
#print frame
#Ashape = np.shape(problem.A)
#if len(Ashape) >0 :
# for i in range(0, Ashape[0]):
# print problem.A[i]
#print problem.A[]
#print problem.b
#print r
problem.clear()
#print r['x']
ype.nested(r['x'], ddth_sol)
#print ddth_sol
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
###########################################
##Jacobian Transpose control
# COM Position control
#fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane)
w1 = 10 #10.1
w2 = 1 #1#2*(w1**.5)
if frame > 100:
w1 = 10.1 #10.1
w2 = 1
footToCMVec = CM - footCenter
desCMPos = [footCenter[0], mm.length(footToCMVec), footCenter[2]]
#print("desCMPos", desCMPos)
#print("CM", CM)
fCom = w1 * (desCMPos - CM) + w2 * (-dCM)
#print("fCom", fCom)
#fCom[0] = 0.
#fCom[1] = 0
#fCom[2] = 0
rd_virtualForce[0] = fCom.copy()
#hipPos = controlModel.getBodyPositionGlobal(rootB)
headPos = controlModel.getBodyPositionGlobal(selectedBody)
hipPos = controlModel.getBodyPositionGlobal(rootB)
yjc.computeJacobian2(Jcom, DOFs, jointPositions, jointAxeses,
[headPos], comUpperJointMasks)
#yjc.computeJacobianDerivative2(dJcom, DOFs, jointPositions, jointAxeses, linkAngVelocities, [CM], comUpperJointMasks, False)
JcomT = Jcom.T
TauJT = np.dot(JcomT, fCom)
# Angular Momentum
Hc = ymt.getAngularMomentum(CM, linkInertias, linkAngVelocities,
linkPositions, linkMasses, linkVelocities)
Href = ymt.getAngularMomentum(CM_ref, linkInertias_ref,
linkAngVelocities_ref, linkPositions_ref,
linkMasses, linkVelocities_ref)
Wam = .05
Tam = Wam * (Href - Hc)
#print("Tam", Tam)
yjc.computeAngJacobian2(JcomAng, DOFs, jointPositions, jointAxeses,
[headPos], comUpperJointMasks)
TauAM = np.dot(JcomAng.T, Tam)
timeReport[5] += time.time() - curTime
curTime = time.time()
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
#print frame, bodyIDs, contactPositions, contactPositionLocals, contactForces
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
#print ddth_sol
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
vpWorld.step()
#if frame%30==0: print 'elapsed time for 30 frames:', time.time()-pt[0]
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - CM_plane)
rd_CM_des[0] = CM_ref_plane.copy()
rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
if (CP is not None) and (dCP is not None):
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
del rd_contactForces[:]
del rd_contactPositions[:]
if CP is not None:
for i in range(len(lcpBodyIDs)):
rd_contactForces.append(lcpContactForces[i].copy() / 200.)
rd_contactPositions.append(lcpContactPositions[i].copy())
timeReport[6] += time.time() - curTime
def main():
np.set_printoptions(precision=4, linewidth=200)
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
motionModel.recordVelByFiniteDiff()
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
footPartNum = config['FootPartNum']
if footPartNum > 1:
elasticity = 2000
damping = 2 * (elasticity**.5)
springBody1 = 5
springBody2 = 6
springBody1Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos) * 1.
node = mcfg.getNode(mit.LEFT_PHALANGE_1)
initialDist -= node.width #0.084
v1 = (-node.width * 0.5, 0.0, node.length * 0.4)
v2 = (node.width * 0.5, 0.0, node.length * 0.4)
controlModel.setSpring(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]),
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]),
elasticity, damping, v2, v1, initialDist)
controlModel.setSpring(
motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]),
motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]),
elasticity, damping, v1, v2, initialDist)
#elasticity = 10
#damping = 2*(elasticity**.5)
#springBody1 = 3
#springBody2 = 4
#node = mcfg.getNode(mit.LEFT_PHALANGE_1)
#springBody1Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
#springBody2Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
#initialDist = mm.length(springBody1Pos - springBody2Pos)*1.
#initialDist -= node.width#0.084
#v1 = (-node.width*0.5,0.0,-node.length*0.4)
#v2 = (node.width*0.5,0.0,-node.length*0.4)
##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#ModelOffset = (1.5, -0.01, 0)
ModelOffset = (1.5, 0.04, 0)
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2'])
#w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
rootB = motion[0].skeleton.getJointIndex(config['root'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex(config['const'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
Jcom = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcom = Jcom.copy()
JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcomAng = JcomAng.copy()
###############
indexFootL = [None] * footPartNum
indexFootR = [None] * footPartNum
jFootL = [None] * footPartNum
dJFootL = [None] * footPartNum
jFootR = [None] * footPartNum
dJFootR = [None] * footPartNum
jointMasksFootL = [None] * footPartNum
jointMasksFootR = [None] * footPartNum
for i in range(footPartNum):
jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootL[i] = jFootL[i].copy()
jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootR[i] = jFootR[i].copy()
indexFootL[i] = motion[0].skeleton.getJointIndex(
config['FootLPart'][i])
indexFootR[i] = motion[0].skeleton.getJointIndex(
config['FootRPart'][i])
jointMasksFootL[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i])
]
jointMasksFootR[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i])
]
constJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton,
[indexFootL[0], indexFootR[0]])
]
#constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])]
#constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks = [
yjc.getLinkJointMask(motion[0].skeleton, selectedBody)
]
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks[0][0] = 0
#comUpperJointMasks[0][1] = 1
#comUpperJointMasks[0][10] = 1
comUpperJointMasks[0][2] = 1
comUpperJointMasks[0][11] = 1
#print(comUpperJointMasks)
comLowerJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton, [
motion[0].skeleton.getJointIndex('LeftLeg'),
motion[0].skeleton.getJointIndex('RightLeg')
])
]
'''
maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks]
parentArray = [supL, supR, supL, supR]
effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR]
for j in range(4) :
for i in range(len(foreSupLJointMasks)) :
if i == parentArray[j] or i == effectorArray[j] :
maskArray[j][0][i] = 1
else :
maskArray[j][0][i] = 0
'''
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0, 0, 0, 0, 0, 0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0, 0, 0, 0, 0, 0] #R
a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM_ref = [None]
rd_CM_des = [None]
rd_CM = [None]
rd_CM_vec = [None]
rd_CM_ref_vec = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_footCenter_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_root = [None]
rd_footL_vec = [None]
rd_footR_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rd_DesForePosL = [None]
rd_DesForePosR = [None]
rd_DesRearPosL = [None]
rd_DesRearPosR = [None]
rd_Joint = [None]
rd_Joint2 = [None]
rd_Joint3 = [None]
rd_Joint4 = [None]
rd_desPoints = [None]
rd_contactForces = [None]
rd_contactPositions = [None]
#rd_contactForces = [None]*10000
#rd_contactPositions = [None]*10000
rd_virtualForce = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0, 0, 0]
normalVector = [[0, 2, 0]]
if MULTI_VIEWER:
viewer = ymv.MultiViewer(800, 655)
#viewer = ymv.MultiViewer(1600, 1255)
viewer.setRenderers1([
cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
viewer.setRenderers2([
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
else:
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (100, 100, 100),
yr.POLYGON_FILL)) #(150,150,255)
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR,
yr.POLYGON_FILL))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_LINE))
#viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
#viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) )
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CM',
yr.PointsRenderer(rd_CM, (255, 0, 255)))
viewer.doc.addRenderer('rd_CM_des',
yr.PointsRenderer(rd_CM_des, (64, 64, 255)))
viewer.doc.addRenderer(
'rd_CM_vec',
yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 128)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001))
viewer.doc.addRenderer(
'rd_exf_des',
yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009,
0.04))
#viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255)))
#viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255)))
#viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255)))
#viewer.doc.addRenderer('rd_DesForePosL', yr.PointsRenderer(rd_DesForePosL, (150,0,200)))
#viewer.doc.addRenderer('rd_DesForePosR', yr.PointsRenderer(rd_DesForePosR, (150,0,250)))
#viewer.doc.addRenderer('rd_DesRearPosL', yr.PointsRenderer(rd_DesRearPosL, (0,150,200)))
#viewer.doc.addRenderer('rd_DesRearPosR', yr.PointsRenderer(rd_DesRearPosR, (0,150,250)))
#viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150,100,100), 3))
#viewer.doc.addRenderer('rd_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255,0,0), 3))
#viewer.doc.addRenderer('rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterR, (255,255,0), 3))
#viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref))
#viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0)))
#viewer.doc.addRenderer('rd_refNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255,0,0), 3))
#viewer.doc.addRenderer('rd_refCMVec', yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255,0,255), 3))
#viewer.doc.addRenderer('rd_curNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter, (255,0,0), 3))
#viewer.doc.addRenderer('rd_CMVec', yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255,0,255), 3))
viewer.doc.addRenderer(
'rd_contactForces',
yr.VectorsRenderer(rd_contactForces, rd_contactPositions,
(0, 255, 0), .1))
#viewer.doc.addRenderer('rd_virtualForce', yr.ForcesRenderer(rd_virtualForce, rd_CM, (50,255,0), 0.5, 0.02))
#viewer.doc.addRenderer('rd_Joint', yr.PointsRenderer(rd_Joint, (255,0,0)))
#viewer.doc.addRenderer('rd_Joint2', yr.PointsRenderer(rd_Joint2, (0,255,0)))
#viewer.doc.addRenderer('rd_Joint3', yr.PointsRenderer(rd_Joint3, (0,0,255)))
#viewer.doc.addRenderer('rd_Joint4', yr.PointsRenderer(rd_Joint4, (255,255,0)))
#viewer.doc.addRenderer('rd_desPoints', yr.PointsRenderer(rd_desPoints, (255,0,0)))
stage = STATIC_BALANCING
contactRendererName = []
#for i in range (motion[0].skeleton.getJointNum()):
# print(i, motion[0].skeleton.getJointName(i))
print "(index, id, name)"
for i in range(controlModel.getBodyNum()):
print(i, controlModel.index2id(i), controlModel.index2name(i))
desCOMOffset = 0.0
pt = [0.]
timeReport = [0.] * 7
viewer.objectInfoWnd.comOffsetY.value(-0.05)
viewer.objectInfoWnd.comOffsetZ.value(0.00)
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc')
viewer.objectInfoWnd.Bc.value(0.1)
viewer.objectInfoWnd.end()
viewer.objectInfoWnd.labelKt.value(50)
viewer.objectInfoWnd.labelKk.value(17)
def simulateCallback(frame):
print "main:frame : ", frame
curTime = time.time()
if frame % 30 == 1: pt[0] = time.time()
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
global desCOMOffset
motionModel.update(motion[0])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
############################
#Reference motion modulation
dCM_k = 10.
linkVelocities = controlModel.getBodyVelocitiesGlobal()
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.08 * np.sin(2 * 3.14 * leftHipTimer / 60.))
#viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.))
#B_Hipd = viewer.objectInfoWnd.labelLeftHip.value()
#newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.)
#idx = motion[0].skeleton.getJointIndex('LeftUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
#idx = motion[0].skeleton.getJointIndex('RightUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
leftHipTimer -= 1
timeReport[0] += time.time() - curTime
curTime = time.time()
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
linkInertias_ref = motionModel.getBodyInertiasGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
timeReport[1] += time.time() - curTime
curTime = time.time()
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
timeReport[2] += time.time() - curTime
curTime = time.time()
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if frame > 0:
dJsys = (Jsys - JsysPre) * 30.
else:
dJsys = (Jsys - Jsys)
JsysPre = Jsys.copy()
timeReport[3] += time.time() - curTime
curTime = time.time()
lcpBodyIDs, lcpContactPositions, lcpContactPositionLocals, lcpContactForces = hls.calcLCPForces(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., 4, None)
# bodyIDs : IDs for Virtual Physics, not VpModel !!!
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if (CP is not None):
CP[1] = 0.
for i in range(controlModel.getBodyNum()):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
for j in range(0, footPartNum):
jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy()
jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy()
dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6] #.copy()
dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6] #.copy()
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = .5 + desCOMOffset
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r +
controlModel.getBodyPositionGlobal(indexFootL[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r +
controlModel.getBodyPositionGlobal(indexFootR[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter = (
getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') +
getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4.
#footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(), 0,
viewer.objectInfoWnd.comOffsetZ.value()
])
#footCenter += footCenterOffset
vecRatio = mm.length(curRelCMVec) * 0.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
######################
# optimization terms
######################
# linear momentum
CM_ref_plane = footCenter + footCenterOffset
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] -
CM[1]) - Dl * totalMass * dCM[1]
#dL_des_plane[1] = 0.
#print 'dL_des_plane', dL_des_plane
# angular momentum
CP_ref = footCenter + footCenterOffset
CP_ref[1] = 0.
timeStep = 30.
if (CP_old[0] is None) or (CP is None):
dCP = None
else:
dCP = (CP - CP_old[0]) * timeStep
CP_old[0] = CP
if (CP is not None) and (dCP is not None):
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#print 'dCP: ', dCP
#print 'ddCP_des: ', ddCP_des
#print 'CP_des: ', CP_des
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross(
(CP_des - CM_plane),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
flagContact = True
if (dH_des is None) or np.any(np.isnan(dH_des)) == True:
flagContact = False
#viewer.doc.showRenderer('rd_grf_des', False)
#viewer.motionViewWnd.update(1, viewer.doc)
#else:
#viewer.doc.showRenderer('rd_grf_des', True)
#viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
#MOTION = FORWARD_JUMP
if mit.MOTION == mit.FORWARD_JUMP:
frame_index = [136, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO2:
frame_index = [130 + 40, 100]
elif mit.MOTION == mit.WALK:
frame_index = [10000, 60]
elif mit.MOTION == mit.TIPTOE:
frame_index = [1000000, 1000000]
#frame_index = [10000, 165]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
if frame > frame_index[0]:
if stage != POWERFUL_BALANCING:
print("#", frame, "-POWERFUL_BALANCING")
stage = POWERFUL_BALANCING
Kk = Kk * 2
Dk = 2 * (Kk**.5)
elif frame > frame_index[1]:
if stage != MOTION_TRACKING:
print("#", frame, "-MOTION_TRACKING")
stage = MOTION_TRACKING
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if 0: #stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt * 2
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
if flagContact == True:
if stage != MOTION_TRACKING + 10:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R,
r_bias)
#mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
# using || dH ||^2 instead
mot.addAnotherTerms(problem, totalDOF, Bh, S,
-(s_bias + Kh * np.dot(S, dth_flat)))
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
# Hard constraint
Kk2 = Kk * 4.0
Dk2 = 2 * (Kk2**.5)
ankleW = 0
ankleOffset = ankleW * curRelCMVec[2]
metatarW = 0
metatarOffset = metatarW * curRelCMVec[2]
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING and frame > 5:
# ankle strategy
idx = 0 #LEFT/RIGHT_TOES
if mit.FOOT_PART_NUM == 1:
yOffset = 0.03
else:
yOffset = 0.069
#yOffset = 0.06
# ankleOffset = (footCenter - CM_plane)*4.
ankleOffset = footCenterOffset * 10.
ankleOffset[1] = 0.
#ankleOffset[2] = 0.
ankleOffset[2] = ankleOffset[2] * 20.
ankleOffsetL = ankleOffset.copy()
ankleOffsetR = ankleOffset.copy()
#ankleOffset= np.array((0,0,0))
if footCenterOffset[0] > 0.0:
ankleOffsetL[0] = 0.
else:
ankleOffsetR[0] = 0.
# print 'ankleOffset=', ankleOffset
desLinearAccL, desPosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[idx], ModelOffset,
CM_ref, CM, Kk, Dk, yOffset) #0.076) #0.14)
desLinearAccR, desPosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[idx], ModelOffset,
CM_ref, CM, Kk, Dk, yOffset)
ax = [0, 0, -1]
aaa = getBodyGlobalOri(controlModel, motion, 'RightFoot')
#print np.dot(aaa, ax)
if mit.FOOT_PART_NUM == 1:
ax = [0, 1, 0]
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[idx], Kk, Dk, ax,
mm.normalize([0, 1, 0] + ankleOffsetL))
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[idx], Kk, Dk, ax,
mm.normalize([0, 1, 0] + ankleOffsetR))
a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)),
np.hstack((desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((jFootL[idx], jFootR[idx]))
dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx]))
#mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.addConstraint(problem, totalDOF, Jsup_2[:1], dJsup_2[:1], dth_flat, a_sup_2[:1])
#mot.addConstraint(problem, totalDOF, Jsup_2[2:], dJsup_2[2:], dth_flat, a_sup_2[2:])
#mot.addConstraint(problem, totalDOF, Jsup_2[3:], dJsup_2[3:], dth_flat, a_sup_2[3:])
mot.addAnotherTerms(problem, totalDOF,
viewer.objectInfoWnd.Bc.value(), Jsup_2[3:],
a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[0:1], a_sup_2[0:1] - np.dot(dJsup_2[0:1] , dth_flat))
#mot.addAnotherTerms(problem, totalDOF, 1.*viewer.objectInfoWnd.Bc.value(), Jsup_2[2:], a_sup_2[2:] - np.dot(dJsup_2[2:] , dth_flat))
desCOMOffset = 0.0
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
if stage == STATIC_BALANCING and frame > 10: # and False:
del rd_desPoints[:]
# foot strategy
#Kk2 = Kk * 2.5
#Kk2 = Kk * .2
#Dk2 = 2*(Kk2**.5)
desForePosL = [0, 0, 0]
desForePosR = [0, 0, 0]
desRearPosL = [0, 0, 0]
desRearPosR = [0, 0, 0]
footPartPos = []
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('LeftCalcaneus_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('LeftPhalange_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightCalcaneus_1')))
footPartPos.append(
controlModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex('RightPhalange_1')))
for i in range(1, footPartNum):
contactFlagFootL[i] = 1
contactFlagFootR[i] = 1
SupPts = np.vstack(
(np.array((footPartPos[0][0], footPartPos[1][0],
footPartPos[2][0], footPartPos[3][0])),
np.array(
(footPartPos[0][2], footPartPos[1][2], footPartPos[2][2],
footPartPos[3][2])), np.array((1., 1., 1., 1.))))
coordWidthLen = 2.
coordLengthLen = 1.5
SupUV = np.vstack(
(np.array((-coordWidthLen, -coordWidthLen, coordWidthLen,
coordWidthLen)),
np.array((-coordLengthLen, coordLengthLen, -coordLengthLen,
coordLengthLen)), np.array((1., 1., 1., 1.))))
SupMap = np.dot(np.dot(SupUV, SupUV.T),
np.linalg.inv(np.dot(SupPts, SupUV.T)))
#print SupMap
desFootCenter = footCenter + footCenterOffset
footCenterPts = np.array((desFootCenter[0], desFootCenter[2], 1))
#print np.dot(SupMap, footCenterPts)
#print np.dot(getBodyGlobalOri(controlModel, motion, 'LeftMetatarsal_1'), np.array((0,1,0)))
CM_plane_2D = np.array((CM[0], CM[2], 1))
# CM_plane_UV = np.dot(SupMap, CM_plane_2D)
CM_plane_UV = np.dot(SupMap, footCenterPts)
# print CM_plane_UV
# for i in range(1, footPartNum):
if CM_plane_UV[1] > .5:
# com is in front
for i in range(1, 5):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
elif CM_plane_UV[1] < -.5:
# com is back
for i in range(3, footPartNum):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
else:
# com is in middle position
for i in range(3, 5):
contactFlagFootL[i] = 0
contactFlagFootR[i] = 0
contactFlagFoot = contactFlagFootL
if CM_plane_UV[0] < 0.:
contactFlagFoot = contactFlagFootR
# CM_plane_UV[0] = -CM_plane_UV[0]
if abs(CM_plane_UV[0]) > 1.:
for j in range(0, 3):
contactFlagFoot[2 * j + 2] = 0
# print 'footL : ',contactFlagFootL
# print 'footR : ',contactFlagFootR
for i in range(1, footPartNum):
axis = [0, 0, 1]
if i == 1 or i == 2:
axis = [0, 0, -1]
desAng = [0, 0, 1]
if i == 1 or i == 2:
desAng = [0, 0, -1]
desY = 0.029
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[i], ModelOffset,
CM_ref, CM, Kk2, Dk2, desY)
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2,
axis, desAng)
a_sup_2 = np.hstack((desLinearAccL, desAngularAccL))
Jsup_2 = jFootL[i].copy()
dJsup_2 = dJFootL[i].copy()
mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2,
dth_flat, a_sup_2)
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:] , dth_flat))
rd_desPoints.append(desForePosL.copy())
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[i], ModelOffset,
CM_ref, CM, Kk2, Dk2, desY)
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2,
axis, desAng)
a_sup_2 = np.hstack((desLinearAccR, desAngularAccR))
Jsup_2 = jFootR[i].copy()
dJsup_2 = dJFootR[i].copy()
mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2,
dth_flat, a_sup_2)
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2, a_sup_2 - np.dot(dJsup_2, dth_flat))
#mot.addAnotherTerms(problem, totalDOF, viewer.objectInfoWnd.Bc.value(), Jsup_2[3:], a_sup_2[3:] - np.dot(dJsup_2[3:], dth_flat))
rd_desPoints.append(desForePosR.copy())
rd_DesForePosL[0] = desForePosL
rd_DesForePosR[0] = desForePosR
rd_DesRearPosL[0] = desRearPosL
rd_DesRearPosR[0] = desRearPosR
##############################
#if Jsup_2 is not None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
r = problem.solve()
#print frame
#Ashape = np.shape(problem.A)
#if len(Ashape) >0 :
# for i in range(0, Ashape[0]):
# print problem.A[i]
#print problem.A[]
#print problem.b
#print r
problem.clear()
#print r['x']
ype.nested(r['x'], ddth_sol)
#print ddth_sol
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
###########################################
##Jacobian Transpose control
# COM Position control
#fCom = Wcp*(pHatComDes - pHatCom) + Wcv*(vComDes - vCom) + Wcm*(footCenter_plane - CM_plane)
w1 = 10 #10.1
w2 = 1 #1#2*(w1**.5)
if frame > 100:
w1 = 10.1 #10.1
w2 = 1
footToCMVec = CM - footCenter
desCMPos = [footCenter[0], mm.length(footToCMVec), footCenter[2]]
#print("desCMPos", desCMPos)
#print("CM", CM)
fCom = w1 * (desCMPos - CM) + w2 * (-dCM)
#print("fCom", fCom)
#fCom[0] = 0.
#fCom[1] = 0
#fCom[2] = 0
rd_virtualForce[0] = fCom.copy()
#hipPos = controlModel.getBodyPositionGlobal(rootB)
headPos = controlModel.getBodyPositionGlobal(selectedBody)
hipPos = controlModel.getBodyPositionGlobal(rootB)
yjc.computeJacobian2(Jcom, DOFs, jointPositions, jointAxeses,
[headPos], comUpperJointMasks)
#yjc.computeJacobianDerivative2(dJcom, DOFs, jointPositions, jointAxeses, linkAngVelocities, [CM], comUpperJointMasks, False)
JcomT = Jcom.T
TauJT = np.dot(JcomT, fCom)
# Angular Momentum
Hc = ymt.getAngularMomentum(CM, linkInertias, linkAngVelocities,
linkPositions, linkMasses, linkVelocities)
Href = ymt.getAngularMomentum(CM_ref, linkInertias_ref,
linkAngVelocities_ref, linkPositions_ref,
linkMasses, linkVelocities_ref)
Wam = .05
Tam = Wam * (Href - Hc)
#print("Tam", Tam)
yjc.computeAngJacobian2(JcomAng, DOFs, jointPositions, jointAxeses,
[headPos], comUpperJointMasks)
TauAM = np.dot(JcomAng.T, Tam)
timeReport[5] += time.time() - curTime
curTime = time.time()
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
#print frame, bodyIDs, contactPositions, contactPositionLocals, contactForces
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
#print ddth_sol
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
vpWorld.step()
#if frame%30==0: print 'elapsed time for 30 frames:', time.time()-pt[0]
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - CM_plane)
rd_CM_des[0] = CM_ref_plane.copy()
rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
if (CP is not None) and (dCP is not None):
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
del rd_contactForces[:]
del rd_contactPositions[:]
if CP is not None:
for i in range(len(lcpBodyIDs)):
rd_contactForces.append(lcpContactForces[i].copy() / 200.)
rd_contactPositions.append(lcpContactPositions[i].copy())
timeReport[6] += time.time() - curTime
# print timeReport
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def ik_analytic(posture, joint_name_or_index, new_position):
if isinstance(joint_name_or_index, int):
joint = joint_name_or_index
else:
joint = posture.skeleton.getJointIndex(joint_name_or_index)
# joint_parent = posture.body.joint_parent[joint]
# joint_parent_parent = posture.body.joint_parent[joint_parent]
joint_parent = posture.skeleton.getParentJointIndex(joint)
joint_parent_parent = posture.skeleton.getParentJointIndex(joint_parent)
# B = posture.get_position(joint)
# C = posture.get_position(joint_parent)
# A = posture.get_position(joint_parent_parent)
B = posture.getJointPositionGlobal(joint)
C = posture.getJointPositionGlobal(joint_parent)
A = posture.getJointPositionGlobal(joint_parent_parent)
L = B - A
N = B - C
M = C - A
# l = mathlib.length(L);
# n = mathlib.length(N);
# m = mathlib.length(M);
l = mm.length(L);
n = mm.length(N);
m = mm.length(M);
# a = mathlib.ACOS((l*l + n*n - m*m) / (2*l*n))
# b = mathlib.ACOS((l*l + m*m - n*n) / (2*l*m))
a = mm.ACOS((l*l + n*n - m*m) / (2*l*n))
b = mm.ACOS((l*l + m*m - n*n) / (2*l*m))
B_new = new_position;
L_new = B_new - A;
# l_ = mathlib.length(L_new)
l_ = mm.length(L_new)
# a_ = mathlib.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
# b_ = mathlib.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
a_ = mm.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
b_ = mm.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
# rotate joint in plane
# rotV = mathlib.normalize(numpy.cross(M, L))
rotV = mm.normalize2(np.cross(M, L))
rotb = b - b_;
rota = a_ - a - rotb;
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV, rotb))
# posture.rotate_global_orientation(joint_parent, mathlib.exp(rotV * rota))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb))
posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota))
# rotate plane
# rotV2 = mathlib.normalize(numpy.cross(L, L_new))
# l_new = mathlib.length(L_new)
# l_diff = mathlib.length(L_new - L)
# rot2 = mathlib.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV2, rot2))
rotV2 = mm.normalize2(np.cross(L, L_new))
l_new = mm.length(L_new)
l_diff = mm.length(L_new - L)
rot2 = mm.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2))
return posture
def _calcPDTorqueJoint(self, joint, parentR, posture, torques):
# R = numpy.dot(parentR, posture.localRMap[joint.name])
R = numpy.dot(
parentR,
posture.localRs[posture.skeleton.getElementIndex(joint.name)])
# if joint.name in self.nodes and joint.parent:
temp_joint = joint
nodeExistParentJoint = None
while True:
if temp_joint.parent == None:
nodeExistParentJoint = None
break
elif temp_joint.parent.name in self.nodes:
nodeExistParentJoint = temp_joint.parent
break
else:
# Gp' * Lc' = Gc' (= Gp)
# Gp' = Gp * inv(Lc')
parentR = numpy.dot(
parentR,
numpy.transpose(
posture.localRs[posture.skeleton.getElementIndex(
temp_joint.parent.name)]))
temp_joint = temp_joint.parent
if joint.name in self.nodes and nodeExistParentJoint:
node = self.nodes[joint.name]
ode_joint = node.joint
if isinstance(ode_joint, ode.FixedJoint) == False:
# Rpd = numpy.dot(parentR, self.boneRs[joint.parent.name]) # parent_desired_SO3
Rpd = numpy.dot(parentR, self.boneRs[
nodeExistParentJoint.name]) # parent_desired_SO3
Rcd = numpy.dot(R,
self.boneRs[joint.name]) # child_desired_SO3
# Rpd = mm.I_SO3()
# Rcd = mm.I_SO3()
# if posture not in self.Rpds:
# self.Rpds[posture] = {}
# if ode_joint not in self.Rpds[posture]:
# Rpd = numpy.dot(parentR, self.boneRs[joint.parent.name]) # parent_desired_SO3
# self.Rpds[posture][ode_joint] = Rpd
# else:
# Rpd = self.Rpds[posture][ode_joint]
#
# if posture not in self.Rpcs:
# self.Rpcs[posture] = {}
# if ode_joint not in self.Rpcs[posture]:
# Rcd = numpy.dot(R, self.boneRs[joint.name]) # child_desired_SO3
# self.Rpcs[posture][ode_joint] = Rcd
# else:
# Rcd = self.Rpcs[posture][ode_joint]
parent = ode_joint.getBody(0)
child = ode_joint.getBody(1)
Rpc = mm.odeSO3ToSO3(
parent.getRotation()) # parent_current_SO3
Rcc = mm.odeSO3ToSO3(child.getRotation()) # child_current_SO3
if isinstance(ode_joint, ode.BallJoint):
Ra = numpy.dot(Rpc, Rpd.transpose()) # align_SO3
Rcd2 = numpy.dot(Ra, Rcd)
# dR = -mm.logSO3_tuple(numpy.dot(Rcd2, Rcc.transpose())) # diff_rot
dR = mm.logSO3(numpy.dot(Rcd2, Rcc.transpose())) # diff_rot
Wpc = parent.getAngularVel()
Wcc = child.getAngularVel()
rW = (-Wpc[0] + Wcc[0], -Wpc[1] + Wcc[1], -Wpc[2] + Wcc[2])
# rW = numpy.array([-Wpc[0]+Wcc[0], -Wpc[1]+Wcc[1], -Wpc[2]+Wcc[2]])
ode_motor = node.motor
ode_motor.setAxis(0, 0, -dR)
ode_motor.setAxis(1, 0, rW)
# ode_motor.setAxis(0, 0, dR+dW)
torques[joint.name] = (node.Kp * (mm.length(dR)),
node.Kd * (mm.length(rW)))
# torques[joint.name] = node.Kp*(mm.length(dR)) + node.Kd*(mm.length(dW))
elif isinstance(ode_joint, ode.HingeJoint):
desiredAngle = mm.logSO3(numpy.dot(Rcd, Rpd.transpose()))
currentAngle = mm.logSO3(numpy.dot(Rcc, Rpc.transpose()))
# desiredAngle = mm.logSO3_old(numpy.dot(Rcd, Rpd.transpose()))
# currentAngle = mm.logSO3_old(numpy.dot(Rcc, Rpc.transpose()))
jointAxis = ode_joint.getAxis()
desiredScala = numpy.inner(jointAxis, desiredAngle)
currentScala = numpy.inner(jointAxis, currentAngle)
# diffAngle = currentScala - desiredScala
diffAngle = mm.diffAngle(currentScala, desiredScala)
# print 'c:', mm.rad2Deg(currentScala), 'd:', mm.rad2Deg(desiredScala)
# print 'diff:', mm.rad2Deg(diffAngle)
# if currentScala * desiredScala < 0:
# diffAngle2 = currentScala - (math.pi*2 - desiredScala)
## diffAngle2 = 2*math.pi - diffAngle
## diffAngle2 = diffAngle - 2*math.pi
## print 'chdiff:', mm.rad2Deg(diffAngle), '->', mm.rad2Deg(diffAngle2)
# diffAngle = diffAngle2
# else:
# diffAngle = currentScala - desiredScala
# print 'diff:', mm.rad2Deg(diffAngle)
angleRate = ode_joint.getAngleRate()
torques[joint.name] = (node.Kp * (diffAngle),
node.Kd * (angleRate))
elif isinstance(ode_joint, ode.UniversalJoint):
desiredAngle = mm.logSO3(numpy.dot(Rcd, Rpd.transpose()))
currentAngle = mm.logSO3(numpy.dot(Rcc, Rpc.transpose()))
jointAxis1 = ode_joint.getAxis1()
desiredScala1 = numpy.inner(jointAxis1, desiredAngle)
currentScala1 = numpy.inner(jointAxis1, currentAngle)
jointAxis2 = ode_joint.getAxis2()
desiredScala2 = numpy.inner(jointAxis2, desiredAngle)
currentScala2 = numpy.inner(jointAxis2, currentAngle)
diffAngle1 = mm.diffAngle(currentScala1, desiredScala1)
diffAngle2 = mm.diffAngle(currentScala2, desiredScala2)
Wpc = parent.getAngularVel()
Wcc = child.getAngularVel()
rW = (-Wpc[0] + Wcc[0], -Wpc[1] + Wcc[1], -Wpc[2] + Wcc[2])
angleRate1 = -numpy.inner(jointAxis1, rW)
angleRate2 = -numpy.inner(jointAxis2, rW)
torques[joint.name] = ((node.Kp * (diffAngle1),
node.Kd * (angleRate1)),
(node.Kp * (diffAngle2),
node.Kd * (angleRate2)))
elif isinstance(ode_joint, ode.FixedJoint):
pass
# print joint.name
# print self.boneRs
# print Rpd
# print Rcd
# print Rpc
# print Rcc
# print dR
# print dW
for childJoint in joint.children:
self._calcPDTorqueJoint(childJoint, R, posture, torques)
CM_vels = [None]*len(intervals)
stepLengths = [None]*len(intervals)
stepDurations = [None]*len(intervals)
print(' CM vel step length step duration')
for i in range(len(intervals)):
startFrame = intervals[i][0]
endFrame = intervals[i][1]
stepDurations[i] = (endFrame - startFrame) * 1/30.
motionModel.update(motion[startFrame])
CM0 = yrp.getCM(motionModel.getBodyPositionsGlobal(), bodyMasses, totalMass)
motionModel.update(motion[endFrame])
CM1 = yrp.getCM(motionModel.getBodyPositionsGlobal(), bodyMasses, totalMass)
CM_vels[i] = mm.length((CM1 - CM0) / stepDurations[i])
lSwingLength = mm.length(motion[endFrame].getJointPositionGlobal(lFoot) - motion[startFrame].getJointPositionGlobal(lFoot))
rSwingLength = mm.length(motion[endFrame].getJointPositionGlobal(rFoot) - motion[startFrame].getJointPositionGlobal(rFoot))
stepLengths[i] = max([lSwingLength, rSwingLength])
if STEP_INFO:
print('%2dth'%i, '%-6s'%yba.GaitState.text[states[i]], '%-10s'%intervals[i], '%2d'%(endFrame-startFrame), '%10.2f %10.2f %10.2f'%(CM_vels[i], stepLengths[i], stepDurations[i]))
startSeg = 1
endSeg = len(intervals)-2
mean_CM_vel = sum(CM_vels[startSeg:endSeg+1]) / (endSeg - startSeg + 1)
mean_stepLength = sum(stepLengths[startSeg:endSeg+1]) / (endSeg - startSeg + 1)
mean_stepDuration = sum(stepDurations[startSeg:endSeg+1]) / (endSeg - startSeg + 1)
print('mean (%dth~%dth) %10.2f %10.2f %10.2f'%(startSeg, endSeg, mean_CM_vel, mean_stepLength, mean_stepDuration))
print()
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
# maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame-1 if frame>0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1))
diff_dCM_axis = np.cross((0,1,0), diff_dCM)
rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0,1,0), diff_CMr)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor)
t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0])
t_raw = t
if t>1.: t=1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot)
R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState!=yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t);
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3()
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
else:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag_faster * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot)
R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot)
if OLD_SWING_HEIGHT:
height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
else:
height_tar = motion_swf_height[prev_frame].getJointPositionGlobal(swingFoot)[1] - groundHeight
d_height_tar = motion_swf_height.getJointVelocityGlobal(swingFoot, prev_frame)[1]
# height_tar += c_swf_mid_offset * swf_height_sine_func(t)
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
if OLD_SWING_HEIGHT:
height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
else:
height_cur = controlModel.getJointPositionGlobal(swingFoot)[1] - halfFootHeight - c_swf_offset
d_height_cur = controlModel.getJointVelocityGlobal(swingFoot)[1]
if OLD_SWING_HEIGHT:
offset_height = (height_tar - height_cur) * swf_height_func(t) * c5
else:
offset_height = ((height_tar - height_cur) * c5
+ (d_height_tar - d_height_cur) * c6) * swf_height_func(t)
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
# offset_sine = 0.
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition)
else:
if HIGHER_OFFSET:
newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot)
if plot!=None:
plot.addDataPoint('debug1', frame, offset_height)
plot.addDataPoint('debug2', frame, height_tar - height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
R_stb = np.dot(R_stb, mm.exp(diff_CMr_axis * K_stb_pos * stf_balancing_func(t)))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0.,0.,0.)
F = mm.v3(0.,0.,0.)
avg_dCM[0] = mm.v3(0.,0.,0.)
# external force rendering info
del rd_forces[:]; del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
rd_forces.append(fi.force)
rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody) + -mm.normalize2(fi.force)*.2)
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
# # apply external force
# for fi in forceInfos:
# if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
# controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState==yba.GaitState.STOP:
if frame == len(motion_seg)-1:
lastFrame = True
elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time:
swingID = lID if curState==yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i]==swingID:
vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel: minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg)-1:
print frame, 'extend frame', frame+1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee]
preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg)-1: lastFrame = True
if lastFrame:
if segIndex < len(segments)-1:
print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval)
if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01)
if len(stanceFoots)>0 and len(swingFoots)>0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0])
step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1))
step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame+1:]
motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False))
del motion_seg[frame+1:]
del motion_stitch[frame+1:]
transitionLength = len(curSeg)-1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False))
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
rd_CP[0] = CP
rd_CMP[0] = (CMreal[0] - (F[0]/F[1])*CMreal[1], 0, CMreal[2] - (F[2]/F[1])*CMreal[1])
if plot!=None:
plot.addDataPoint('zero', frame, 0)
plot.updatePoints()
plot.addYdata('clFoot', clFoot, False)
plot.addYdata('crFoot', crFoot, False)
# plot.addYdata('gaitStates', gaitStates, False)
# plot.addYdata('gaitStates1', gaitStates1, False)
# plot.addYdata('gaitStates0', gaitStates0, False)
plot.addYdata('lFootStates', lFootStates, False)
plot.addYdata('rFootStates', rFootStates, False)
plot.addYdata(
'lPos.y',
[motion.getPosition(LFOOT, i)[1] for i in range(len(motion))], False)
plot.addYdata(
'rPos.y',
[motion.getPosition(RFOOT, i)[1] for i in range(len(motion))], False)
plot.addYlines('hRef', [hRef], False)
plot.addYdata('lVel.length', [
mmMath.length(motion.getVelocity(LFOOT, i)) for i in range(len(motion))
], False)
plot.addYdata('rVel.length', [
mmMath.length(motion.getVelocity(RFOOT, i)) for i in range(len(motion))
], False)
plot.addYlines('vRef', [vRef], False)
plot.showModeless()
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (100, 255, 100),
yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
# lSwingRenderer = yr.JointMotionRenderer(motion, (255,153,0), yr.LINK_BONE)
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
motionModel.recordVelByFiniteDiff()
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
elasticity = 20000
damping = 2*(elasticity**.5)
springBody1 = 1
springBody2 = 2
springBody1Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos)*1.
node = mcfg.getNode(mit.RIGHT_METATARSAL_1)
initialDist -= node.width#0.084
v1 = (-node.width*0.5,0.0,node.length*0.4)
v2 = (node.width*0.5,0.0,node.length*0.4)
controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
v1 = (-node.width*0.5,0.0,-node.length*0.4)
v2 = (node.width*0.5,0.02,-node.length*0.4)
controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
elasticity2 = 10000
damping2 = 2*(elasticity**.5)
springBody3 = 5
springBody4 = 6
springBody3Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody3]))
springBody4Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody4]))
initialDist2 = mm.length(springBody3Pos - springBody4Pos)*1.
node2 = mcfg.getNode(mit.RIGHT_CALCANEUS_1)
initialDist2 -= node2.width
v3 = (-node2.width*0.5,0.0,-node2.length*0.4)
v4 = (node2.width*0.5,0.0,-node2.length*0.4)
controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody3]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody4]), elasticity2, damping2, v4, v3, initialDist2)
controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody3]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody4]), elasticity2, damping2, v3, v4, initialDist2)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#ModelOffset = (1.5, -0.01, 0)
ModelOffset = (1.5, 0.1, 0)
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']; Dt = config['Dt'] # tracking gain
Kl = config['Kl']; Dl = config['Dl'] # linear balance gain
Kh = config['Kh']; Dh = config['Dh'] # angular balance gain
Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2'])
#w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
rootB = motion[0].skeleton.getJointIndex(config['root'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex(config['const'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
###############
footPartNum = config['FootPartNum']
indexFootL = [None]*footPartNum
indexFootR = [None]*footPartNum
jFootL = [None]*footPartNum
dJFootL = [None]*footPartNum
jFootR = [None]*footPartNum
dJFootR = [None]*footPartNum
jointMasksFootL = [None]*footPartNum
jointMasksFootR = [None]*footPartNum
jAngFootL = [None]*footPartNum
dJAngFootL = [None]*footPartNum
jAngFootR = [None]*footPartNum
dJAngFootR = [None]*footPartNum
for i in range(footPartNum) :
jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootL[i] = jFootL[i].copy()
jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootR[i] = jFootR[i].copy()
jAngFootL[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootL[i] = jAngFootL[i].copy()
jAngFootR[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootR[i] = jAngFootR[i].copy()
indexFootL[i] = motion[0].skeleton.getJointIndex(config['FootLPart'][i])
indexFootR[i] = motion[0].skeleton.getJointIndex(config['FootRPart'][i])
jointMasksFootL[i] = [yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i])]
jointMasksFootR[i] = [yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i])]
constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[1], indexFootR[1]])]
#constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])]
#constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
'''
maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks]
parentArray = [supL, supR, supL, supR]
effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR]
for j in range(4) :
for i in range(len(foreSupLJointMasks)) :
if i == parentArray[j] or i == effectorArray[j] :
maskArray[j][0][i] = 1
else :
maskArray[j][0][i] = 0
'''
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0,0,0, 0,0,0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0,0,0, 0,0,0]#R
a_sup_2 = [0,0,0, 0,0,0, 0,0,0, 0,0,0]
CP_old = [mm.v3(0.,0.,0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.]*len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM_ref = [None]
rd_CM = [None]
rd_CM_vec = [None]
rd_CM_ref_vec = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_footCenter_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_root = [None]
rd_footL_vec = [None]
rd_footR_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rd_DesForePosL = [None]
rd_DesForePosR = [None]
rd_DesRearPosL = [None]
rd_DesRearPosR = [None]
rd_Joint = [None]
rd_Joint2 = [None]
rd_Joint3 = [None]
rd_Joint4 = [None]
rd_contactForces = [None]*1000
rd_contactPositions = [None]*1000
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0,0,0]
normalVector = [[0,2,0]]
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) )
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
#viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
viewer.doc.addRenderer('rd_CM', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0)))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001))
viewer.doc.addRenderer('rd_exf_des', yr.ForcesRenderer(rd_exf_des, rd_root_des, (0,255,0), .009, 0.05))
viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255)))
viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255)))
viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255)))
#viewer.doc.addRenderer('rd_DesForePosL', yr.PointsRenderer(rd_DesForePosL, (150,0,200)))
#viewer.doc.addRenderer('rd_DesForePosR', yr.PointsRenderer(rd_DesForePosR, (150,0,250)))
#viewer.doc.addRenderer('rd_DesRearPosL', yr.PointsRenderer(rd_DesRearPosL, (0,150,200)))
#viewer.doc.addRenderer('rd_DesRearPosR', yr.PointsRenderer(rd_DesRearPosR, (0,150,250)))
viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150,100,100), 3))
viewer.doc.addRenderer('rd_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255,0,0), 3))
viewer.doc.addRenderer('rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterR, (255,255,0), 3))
#viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref))
#viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0)))
viewer.doc.addRenderer('rd_refNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255,0,0), 3))
viewer.doc.addRenderer('rd_refCMVec', yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255,0,255), 3))
viewer.doc.addRenderer('rd_curNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter, (255,0,0), 3))
viewer.doc.addRenderer('rd_CMVec', yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255,0,255), 3))
#viewer.doc.addRenderer('rd_contactForces', yr.ForcesRenderer(rd_contactForces, rd_contactPositions, (0,255,0), .009, 0.009))
viewer.doc.addRenderer('rd_Joint', yr.PointsRenderer(rd_Joint, (255,0,0)))
viewer.doc.addRenderer('rd_Joint2', yr.PointsRenderer(rd_Joint2, (0,255,0)))
viewer.doc.addRenderer('rd_Joint3', yr.PointsRenderer(rd_Joint3, (0,0,255)))
viewer.doc.addRenderer('rd_Joint4', yr.PointsRenderer(rd_Joint4, (255,255,0)))
stage = STATIC_BALANCING
contactRendererName = []
for i in range (motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
def simulateCallback(frame):
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
motionModel.update(motion[frame])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam()
Dt = 2*(Kt**.5)
Dk = 2*(Kk**.5)
Dl = 2*(Kl**.5)
Dh = 2*(Kh**.5)
Dsc = 2*(Ksc**.5)
if Bsc == 0.0 :
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1+Bsc*3)
viewer.motionViewWnd.update(1, viewer.doc)
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None]*footPartNum
positionFootR = [None]*footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM); CM_plane[1]=0.
dCM_plane = copy.copy(dCM); dCM_plane[1]=0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0])
yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False)
yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0])
yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False)
yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions, jointAxeses, [positionFootL[0]], jointMasksFootL[0])
yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[0]], jointMasksFootL[0], False)
yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions, jointAxeses, [positionFootR[0]], jointMasksFootR[0])
yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[0]], jointMasksFootR[0], False)
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)) :
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0)
##########################################
for i in range(len(rd_contactPositions)):
rd_contactPositions[i] = [0,0,0]
rd_contactForces[i] = [0,0,0]
for i in range(len(contactPositions)):
rd_contactPositions[i] = np.copy(contactPositions[i])
rd_contactForces[i] = np.copy(contactForces[i])
'''
if len(contactPositions) > 0:
rd_contactPositions = np.copy(contactPositions)
rd_contactForces = np.copy(contactForces)
print("rd_contactPositions", rd_contactPositions)
print("contactPositions", contactPositions)
'''
'''
for i in range(len(contactRendererName)):
viewer.doc.removeRenderer(contactRendererName[i])
del contactRendererName[:]
for i in range(len(contactPositions)):
contactRendererName.append(str(i))
#viewer.doc.addRenderer(str(i), yr.PointsRenderer([contactPositions[i]], (0,255,0)))
viewer.doc.addRenderer(str(i), yr.ForcesRenderer([contactForces[i]], [contactPositions[i]], (0,255,0), .009, 0.009))
viewer.motionViewWnd.update(1, viewer.doc)
'''
##########################################
contactFlagFootL = [0]*footPartNum
contactFlagFootR = [0]*footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)) :
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
if j != 0:
yjc.computePartialJacobian2(jFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j], partialDOFIndex)
yjc.computePartialJacobianDerivative2(dJFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False, partialDOFIndex)
break
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
if j != 0:
yjc.computePartialJacobian2(jFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j], partialDOFIndex)
yjc.computePartialJacobianDerivative2(dJFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False, partialDOFIndex)
break
for j in range(len(indexFootL)):
yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions, jointAxeses, [positionFootL[j]], jointMasksFootL[j])
yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootL[j]], jointMasksFootL[j], False)
yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions, jointAxeses, [positionFootR[j]], jointMasksFootR[j])
yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs, jointPositions, jointAxeses, linkAngVelocities, [positionFootR[j]], jointMasksFootR[j], False)
'''
if footPartNum == 1:
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
else:
if ((contactFlagFootL[3] == 1 or contactFlagFootL[4] == 1) and contactFlagFootL[0] == 0) or ((contactFlagFootR[3] == 1 or contactFlagFootR[4] == 1) and contactFlagFootR[0] == 0):
r = 0.8
footCenterL = (controlModel.getBodyPositionGlobal(supL)*r + controlModel.getBodyPositionGlobal(indexFootL[1])*(1.0-r))
footCenterR = (controlModel.getBodyPositionGlobal(supR)*r + controlModel.getBodyPositionGlobal(indexFootR[1])*(1.0-r))
#footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
#footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
else :
#footCenterL = (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1]))/2.0
#footCenterR = (controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1]))/2.0
#footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
#footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
r = 0.8
footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))
footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r))
'''
'''
if stage == POWERFUL_BALANCING:
footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
else:
footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1]) + controlModel.getBodyPositionGlobal(indexFootL[3]) )/2.0
footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1]) + controlModel.getBodyPositionGlobal(indexFootR[3]))/2.0
'''
'''
p1 = controlModel.getBodyPositionGlobal(indexFootL[0])
p2 = controlModel.getBodyPositionGlobal(indexFootR[0])
p3 = controlModel.getBodyPositionGlobal(indexFootL[3])
p4 = controlModel.getBodyPositionGlobal(indexFootR[3])
print(frame, "supL", p1[1])
print(frame, "supR", p2[1])
print(frame, "calcL", p3[1])
print(frame, "calcR", p4[1])
'''
#footCenter = footCenterL + (footCenterR - footCenterL)/2.0
#footCenter[1] = 0.
#
'''
if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1:
footCenter = footCenter
elif checkAll(contactFlagFootL, 0) == 1 :
footCenter = footCenterR
elif checkAll(contactFlagFootR, 0) == 1 :
footCenter = footCenterL
'''
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else :
r = .4
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r))#controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL)/2.0
if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1:
desFC = desFC
elif checkAll(contactFlagFootL, 0) == 1 :
desFC = desFCR
elif checkAll(contactFlagFootR, 0) == 1 :
desFC = desFCL
#if stage == MOTION_TRACKING:
# desFC = desFCL
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec)*0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec*(vecRatio)#/10.0
footCenter_ref = refFootL + (refFootR - refFootL)/2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
vecRatio = mm.length(curRelCMVec)*0.
softConstPointOffset = -curRelCMVec*(vecRatio)#/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0,0,0]
desForeSupRAcc = [0,0,0]
totalNormalForce = [0,0,0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
print("totalNormalForce=", totalNormalForce[1])
print("F_Diff=", (totalMass*mm.s2v(wcfg.gravity))[1]+totalNormalForce[1])
# linear momentum
CM_ref_plane = footCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
print("CM_Diff=",mm.length(CM_ref_plane - CM_plane))
# angular momentum
CP_ref = footCenter
timeStep = 30.
if CP_old[0]==None or CP==None:
dCP = None
else:
dCP = (CP - CP_old[0])/(1/timeStep)
CP_old[0] = CP
if CP!=None and dCP!=None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des = CP + dCP*(1/timeStep) + .5*ddCP_des*((1/timeStep)**2)
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
print("CP_Diff=",mm.length(CP_des - CP))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
##############################
# soft point constraint
#####################################################
P_cur = controlModel.getBodyPositionGlobal(constBody)
constBodyVec = P_cur - footCenter
softConstPoint = [footCenter[0]+softConstPointOffset[0], mm.length(constBodyVec), footCenter[2]+softConstPointOffset[2]]
#####################################################
P_des = softConstPoint
#P_cur = controlModel.getBodyPositionGlobal(constBody)
dP_des = [0, 0, 0]
dP_cur = controlModel.getBodyVelocityGlobal(constBody)
ddP_des1 = Ksc*(-(P_des - P_cur)) - Dsc*(-(dP_cur - dP_des))
r = -(P_des - P_cur)
I = np.vstack(([1,0,0],[0,1,0],[0,0,1]))
Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses, [P_cur], constJointMasks)
JL, JA = np.vsplit(Jconst, 2)
Q1 = np.dot(Z, Jconst)
q1 = np.dot(JA, dth_flat)
q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r))
yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [P_cur], constJointMasks, False)
q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
##############################
flagContact = True
if dH_des==None or np.any(np.isnan(dH_des)) == True:
flagContact = False
viewer.doc.showRenderer('rd_grf_des', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('rd_grf_des', True)
viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
#MOTION = FORWARD_JUMP
if mit.MOTION == mit.FORWARD_JUMP :
frame_index = [136, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO2:
frame_index = [130+40, 100]
elif mit.MOTION == mit.WALK:
frame_index = [10000, 60]
else :
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
'''
if frame > 300 :
if stage != DYNAMIC_BALANCING:
print("#", frame,"-DYNAMIC_BALANCING")
stage = DYNAMIC_BALANCING
Kk = Kk*1
Dk = 2*(Kk**.5)
'''
if frame > frame_index[0] :
if stage != POWERFUL_BALANCING:
print("#", frame,"-POWERFUL_BALANCING")
stage = POWERFUL_BALANCING
Kk = Kk*2
Dk = 2*(Kk**.5)
elif frame > frame_index[1]:
if stage != MOTION_TRACKING:
print("#", frame,"-MOTION_TRACKING")
stage = MOTION_TRACKING
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt*2
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
if flagContact == True:
if stage != MOTION_TRACKING+10:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = [None]
Jsup_2 = [None]
dJsup_2 = [None]
##############################
# Hard constraint
if stage != MOTION_TRACKING:
Kk2 = Kk * 2.0
else :
Kk2 = Kk * 1.5
Dk2 = 2*(Kk2**.5)
'''
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2)
desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2)
'''
if stage != MOTION_TRACKING:
idx = 0 #LEFT/RIGHT_TOES
if stage != MOTION_TRACKING:
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14)#0.076)
desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.14)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5])
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2, [0,0,-1], [0,1,1.5])
a_sup_2 = np.hstack(( np.hstack((desLinearAccL, desAngularAccL)), np.hstack((desLinearAccR, desAngularAccR)) ))
Jsup_2 = np.vstack((jFootL[idx], jFootR[idx]))
dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx]))
else:
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040)
desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.040)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack((desLinearAccL, desAngularAccL))
Jsup_2 = jFootL[idx]
dJsup_2 = dJFootL[idx]
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
else:
if footPartNum != 5:
idx = 0
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[idx], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.045)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( desLinearAccL, desAngularAccL))
Jsup_2 = (jFootL[idx])
dJsup_2 = (dJFootL[idx])
'''
idx = 4
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, desAngularAccL))
Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx]))
dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx]))
'''
'''
idx = 1
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, desAngularAccL))
Jsup_2 = np.vstack(( Jsup_2, jAngFootL[idx]))
dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[idx]))
'''
else:
idx = 0
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( desAngularAccL, desAngularAccR ))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING+10:
#Kk2 = Kk * 2.5
Kk2 = Kk * 2.5
Dk2 = 2*(Kk2**.5)
desForePosL = [0,0,0]
desForePosR = [0,0,0]
desRearPosL = [0,0,0]
desRearPosR = [0,0,0]
for i in range(1, footPartNum) :
if stage != MOTION_TRACKING:
axis = [0,1,0]
desAng = [0,1,0]
desY = 0.04
if i == 1 or i == 2:
desAng = [0,1,1.2]
desY = 0.076
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2, axis, desAng)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootL[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] ))
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, desY)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2, axis, desAng)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootR[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] ))
else:
if contactFlagFootL[i] == 1:
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, desAngularAccL ))
Jsup_2 = np.vstack(( Jsup_2, jAngFootL[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJAngFootL[i] ))
if contactFlagFootR[i] == 1:
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, desAngularAccR ))
Jsup_2 = np.vstack(( Jsup_2, jAngFootR[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJAngFootR[i] ))
'''
for i in range(1, footPartNum) :
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootL[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] ))
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootR[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] ))
'''
rd_DesForePosL[0] = desForePosL
rd_DesForePosR[0] = desForePosR
rd_DesRearPosL[0] = desRearPosL
rd_DesRearPosR[0] = desRearPosR
##############################
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
rd_Joint[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_1'))
rd_Joint2[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftMetatarsal_3'))
rd_Joint3[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_1'))
rd_Joint4[0] = controlModel.getJointPositionGlobal(motion[0].skeleton.getJointIndex('LeftPhalange_3'))
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) :
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame*wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
'''
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) :
forceApplyFrame += 1
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame*wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
'''
vpWorld.step()
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref)*3.
rd_CM_vec[0] = (CM - footCenter)*3
#rd_CM_plane[0][1] = 0.
if CP!=None and dCP!=None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[0] = totalNormalForce# - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint
#indexL = motion[0].skeleton.getJointIndex('Hips')
#indexR = motion[0].skeleton.getJointIndex('Spine1')
indexL = indexFootL[0]
indexR = indexFootR[0]
curAng = [controlModel.getBodyOrientationGlobal(indexL)]
curAngY = np.dot(curAng, np.array([0,0,1]))
rd_footL_vec[0] = np.copy(curAngY[0])
rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL)
curAng = [controlModel.getBodyOrientationGlobal(indexR)]
curAngY = np.dot(curAng, np.array([0,0,1]))
rd_footR_vec[0] = np.copy(curAngY[0])
rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR)
if (forceApplyFrame == 0) :
applyedExtraForce[0] = [0, 0, 0]
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/60.)
viewer.show()
Fl.run()
from fltk import *
import sys
sys.path.append('../PyCommon/modules')
import Math.mmMath as mmMath
import Resource.ysMotionLoader as yf
import Renderer.ysRenderer as yr
import GUI.ysSimpleViewer as ysv
if __name__ == "__main__":
motion0, frameTime = yf.readBvhFileAsJointMotion(
'Data/wd2_WalkSameSame00.bvh', .01)
motion1, frameTime = yf.readBvhFileAsJointMotion(
'Data/wd2_spiral_walk_normal05.bvh', .01)
length0 = mmMath.length(motion0[0].getGlobalPos('RightUpLeg') -
motion0[0].getGlobalPos('RightLeg'))
length1 = mmMath.length(motion1[0].getGlobalPos('RightUpLeg') -
motion1[0].getGlobalPos('RightLeg'))
scale = length0 / length1
print 'scale', scale
scaledMotion1, frameTime = yf.readBvhFileAsJointMotion(
'Data/wd2_spiral_walk_normal05.bvh', .01 * scale)
viewer = ysv.SimpleViewer()
viewer.record(False)
viewer.doc.addRenderer(
'motion0', yr.JointMotionRenderer(motion0, (255, 0, 0), yr.LINK_BONE))
viewer.doc.addRenderer(
'motion1', yr.JointMotionRenderer(motion1, (0, 0, 255), yr.LINK_BONE))
viewer.doc.addRenderer(
def _createBody(self, joint, parentT, posture):
T = parentT
P = mm.TransVToSE3(joint.offset)
T = numpy.dot(T, P)
# R = mm.SO3ToSE3(posture.localRMap[joint.name])
R = mm.SO3ToSE3(posture.localRs[posture.skeleton.getElementIndex(
joint.name)])
T = numpy.dot(T, R)
if len(joint.children) > 0 and joint.name in self.config.nodes:
offset = numpy.array([0., 0., 0.])
for childJoint in joint.children:
offset += childJoint.offset
offset = offset / len(joint.children)
boneT = mm.TransVToSE3(offset / 2.)
defaultBoneV = numpy.array([0, 0, 1])
boneR = mm.getSO3FromVectors(defaultBoneV, offset)
self.boneRs[joint.name] = boneR
boneT = numpy.dot(boneT, mm.SO3ToSE3(boneR))
node = OdeModel.Node(joint.name)
self.nodes[joint.name] = node
node.body = ode.Body(self.world)
mass = ode.Mass()
cfgNode = self.config.getNode(joint.name)
if cfgNode.length:
length = cfgNode.length * cfgNode.boneRatio
else:
length = mm.length(offset) * cfgNode.boneRatio
if cfgNode.width:
width = cfgNode.width
if cfgNode.mass:
height = (cfgNode.mass /
(cfgNode.density * length)) / width
else:
height = .1
else:
if cfgNode.mass:
width = (cfgNode.mass / (cfgNode.density * length))**.5
else:
width = .1
height = width
node.geom = ode.GeomBox(self.space, (width, height, length))
node.geom.name = joint.name
mass.setBox(cfgNode.density, width, height, length)
boneT = numpy.dot(boneT, mm.TransVToSE3(cfgNode.offset))
self.boneTs[joint.name] = boneT
newT = numpy.dot(T, boneT)
p = mm.SE3ToTransV(newT)
r = mm.SE3ToSO3(newT)
node.geom.setBody(node.body)
node.body.setMass(mass)
node.body.setPosition(p)
node.body.setRotation(mm.SO3ToOdeSO3(r))
for childJoint in joint.children:
self._createBody(childJoint, T, posture)
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame-1 if frame>0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1))
diff_dCM_axis = np.cross((0,1,0), diff_dCM)
rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0,1,0), diff_CMr)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor)
t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0])
t_raw = t
if t>1.: t=1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot)
R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState!=yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t);
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3()
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot)
R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot)
height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
offset_height = (height_tar - height_cur) * swf_height_func(t) * c_swf_stability
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition)
else:
newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot)
if plot!=None:
plot.addDataPoint('debug1', frame, height_tar)
plot.addDataPoint('debug2', frame, height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
# swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0.,0.,0.)
F = mm.v3(0.,0.,0.)
avg_dCM[0] = mm.v3(0.,0.,0.)
# external force rendering info
del rd_forces[:]; del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
rd_forces.append(fi.force)
rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody))
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
if TORQUE_PLOT:
rhip_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rUpLeg))
rknee_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rKnee))
rankle_torques[frame] += mm.length(controlModel.getJointTorqueLocal(rFoot))
rd_torques[:] = [controlModel.getJointTorqueLocal(i)/100. for i in range(skeleton.getJointNum())]
rd_joint_positions[:] = controlModel.getJointPositionsGlobal()
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState==yba.GaitState.STOP:
if frame == len(motion_seg)-1:
lastFrame = True
elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time:
swingID = lID if curState==yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i]==swingID:
vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel: minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg)-1:
print frame, 'extend frame', frame+1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee]
preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg)-1: lastFrame = True
if lastFrame:
if segIndex < len(segments)-1:
print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval)
if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01)
if len(stanceFoots)>0 and len(swingFoots)>0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0])
step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1))
step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame+1:]
motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False))
del motion_seg[frame+1:]
del motion_stitch[frame+1:]
transitionLength = len(curSeg)-1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False))
if NO_FOOT_SLIDING:
if segIndex == len(segments)-2:
Rl = motion_control[-1].getJointOrientationLocal(lUpLeg)
Rr = motion_control[-1].getJointOrientationLocal(rUpLeg)
Rlk = motion_control[-1].getJointOrientationLocal(lKnee)
Rrk = motion_control[-1].getJointOrientationLocal(rKnee)
Rlf = motion_control[-1].getJointOrientationLocal(lFoot)
Rrf = motion_control[-1].getJointOrientationLocal(rFoot)
for p in curSeg:
p.setJointOrientationLocal(lUpLeg, Rl, False)
p.setJointOrientationLocal(rUpLeg, Rr, False)
p.setJointOrientationLocal(lKnee, Rlk, False)
p.setJointOrientationLocal(rKnee, Rrk, False)
p.setJointOrientationLocal(lFoot, Rlf, False)
p.setJointOrientationLocal(rFoot, Rrf, False)
p.updateGlobalT()
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
if SAVE_SIMULATION:
p_temp = ym.JointPosture(skeleton)
p_temp.initLocalRs()
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
motion_simulation.append(p_temp)
if frame == viewer.getMaxFrame():
saveFilePath = SAVE_DIR+'simulated_'+filename
yf.writeBvhFile(saveFilePath, motion_simulation)
print saveFilePath, 'saved'
def simulateCallback(frame):
curTime = time.time()
if frame % 30 == 1: pt[0] = time.time()
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
global desCOMOffset
#motionModel.update(motion[0])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
'''
if Bsc == 0.0 :
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1+Bsc*3)
viewer.motionViewWnd.update(1, viewer.doc)
'''
pose = motion[0].copy()
def solveIK(desComPos,
desIdxs,
desPos,
desOri,
cmW=10.,
posW=1.,
oriW=1.):
numItr = 100
dt = .5
threshold = 0.1
for i in range(0, numItr):
jPart_IK = []
print '----iter num', i
IKModel.update(pose)
th_r_IK = pose.getDOFPositions()
jointPositions_IK = pose.getJointPositionsGlobal()
jointAxeses_IK = pose.getDOFAxeses()
linkPositions_IK = IKModel.getBodyPositionsGlobal()
linkInertias_IK = IKModel.getBodyInertiasGlobal()
CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass)
print CM_IK
P_IK = ymt.getPureInertiaMatrix(TO, linkMasses,
linkPositions_IK, CM_IK,
linkInertias_IK)
yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK,
jointAxeses_IK, linkPositions_IK,
allLinkJointMasks)
for j in range(0, len(desIdxs)):
jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6])
J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2)
dv_IK = cmW * (desComPos - CM_IK)
for j in range(0, len(desIdxs)):
J_IK = np.vstack((J_IK, jPart_IK[j]))
pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j])
dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK))
ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j])
dv_IK = np.append(dv_IK,
oriW * mm.logSO3(desOri[j] * ori_IK.T))
#print dv_IK[0:3]
dth_IK_solve = npl.lstsq(J_IK, dv_IK)
dth_IK_x = dth_IK_solve[0][:totalDOF]
ype.nested(dth_IK_x, dth_IK)
#print dth_IK[0][0:3]
th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt)
pose.setDOFPositions(th_IK)
if np.dot(dv_IK, dv_IK) < threshold:
break
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
#CM_IK_ref = footCenter + footCenterOffset
CM_IK_ref = CM_ref + footCenterOffset
#CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1]
motion[0].skeleton.getJointIndex(config['supLink'])
#IKidxs = [indexFootL[0], indexFootR[0]]
#IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])]
#for i in range(0, 2):
# #IKdesPos[i] += ModelOffset
# IKdesPos[i][1] = 0.069
#IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])]
#IKdesOri = [None]*2
#for i in range(0, 2):
# IKdesOri[i] = mm.I_SO3()
IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4]
print IKidxs
IKdesPos = [None] * len(IKidxs)
IKdesOri = [None] * len(IKidxs)
for i in range(0, len(IKidxs)):
#print i
IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i])
IKdesPos[i][1] = 0.03
IKdesOri[i] = mm.I_SO3()
print IKdesPos
solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.))
#viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.))
#B_Hipd = viewer.objectInfoWnd.labelLeftHip.value()
#newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.)
#idx = motion[0].skeleton.getJointIndex('LeftUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
#idx = motion[0].skeleton.getJointIndex('RightUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
leftHipTimer -= 1
timeReport[0] += time.time() - curTime
curTime = time.time()
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
linkInertias_ref = motionModel.getBodyInertiasGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
timeReport[1] += time.time() - curTime
curTime = time.time()
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
timeReport[2] += time.time() - curTime
curTime = time.time()
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if frame > 0:
dJsys = (Jsys - JsysPre) * 30.
else:
dJsys = (Jsys - Jsys)
JsysPre = Jsys.copy()
timeReport[3] += time.time() - curTime
curTime = time.time()
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
for j in range(0, footPartNum):
jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy()
jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy()
dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6] #.copy()
dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6] #.copy()
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = .5 + desCOMOffset
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r +
controlModel.getBodyPositionGlobal(indexFootL[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r +
controlModel.getBodyPositionGlobal(indexFootR[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter = (
getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') +
getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4.
#footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(), 0,
viewer.objectInfoWnd.comOffsetZ.value()
])
#footCenter += footCenterOffset
vecRatio = mm.length(curRelCMVec) * 0.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
######################
# optimization terms
######################
# linear momentum
CM_ref_plane = footCenter + footCenterOffset
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] -
CM[1]) - Dl * totalMass * dCM[1]
# angular momentum
CP_ref = footCenter + footCenterOffset
timeStep = 30.
if CP_old[0] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / timeStep)
CP_old[0] = CP
if CP != None and dCP != None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross(
(CP_des - CM_plane),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
flagContact = True
if dH_des == None or np.any(np.isnan(dH_des)) == True:
flagContact = False
#viewer.doc.showRenderer('rd_grf_des', False)
#viewer.motionViewWnd.update(1, viewer.doc)
#else:
#viewer.doc.showRenderer('rd_grf_des', True)
#viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if 0: #stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt * 2
#######################
# optimization
#######################
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
#if flagContact == True:
# mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
# mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
#if Jsup_2 != None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
timeReport[5] += time.time() - curTime
curTime = time.time()
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
vpWorld.step()
if frame % 30 == 0:
print 'elapsed time for 30 frames:', time.time() - pt[0]
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - CM_plane)
rd_CM_des[0] = CM_ref_plane.copy()
rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
if CP != None and dCP != None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
#indexL = motion[0].skeleton.getJointIndex('Hips')
#indexR = motion[0].skeleton.getJointIndex('Spine1')
indexL = indexFootL[0]
indexR = indexFootR[0]
curAng = [controlModel.getBodyOrientationGlobal(indexL)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footL_vec[0] = np.copy(curAngY[0])
rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL)
curAng = [controlModel.getBodyOrientationGlobal(indexR)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footR_vec[0] = np.copy(curAngY[0])
rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR)
if (forceApplyFrame == 0):
applyedExtraForce[0] = [0, 0, 0]
timeReport[6] += time.time() - curTime
def ik_analytic(posture, joint_name_or_index, new_position):
if isinstance(joint_name_or_index, int):
joint = joint_name_or_index
else:
joint = posture.skeleton.getJointIndex(joint_name_or_index)
# joint_parent = posture.body.joint_parent[joint]
# joint_parent_parent = posture.body.joint_parent[joint_parent]
joint_parent = posture.skeleton.getParentJointIndex(joint)
joint_parent_parent = posture.skeleton.getParentJointIndex(joint_parent)
# B = posture.get_position(joint)
# C = posture.get_position(joint_parent)
# A = posture.get_position(joint_parent_parent)
B = posture.getJointPositionGlobal(joint)
C = posture.getJointPositionGlobal(joint_parent)
A = posture.getJointPositionGlobal(joint_parent_parent)
L = B - A
N = B - C
M = C - A
# l = mathlib.length(L);
# n = mathlib.length(N);
# m = mathlib.length(M);
l = mm.length(L)
n = mm.length(N)
m = mm.length(M)
# a = mathlib.ACOS((l*l + n*n - m*m) / (2*l*n))
# b = mathlib.ACOS((l*l + m*m - n*n) / (2*l*m))
a = mm.ACOS((l * l + n * n - m * m) / (2 * l * n))
b = mm.ACOS((l * l + m * m - n * n) / (2 * l * m))
B_new = new_position
L_new = B_new - A
# l_ = mathlib.length(L_new)
l_ = mm.length(L_new)
# a_ = mathlib.ACOS((l_*l_ + n*n - m*m) / (2*l_*n))
# b_ = mathlib.ACOS((l_*l_ + m*m - n*n) / (2*l_*m))
a_ = mm.ACOS((l_ * l_ + n * n - m * m) / (2 * l_ * n))
b_ = mm.ACOS((l_ * l_ + m * m - n * n) / (2 * l_ * m))
# rotate joint in plane
# rotV = mathlib.normalize(numpy.cross(M, L))
rotV = mm.normalize2(np.cross(M, L))
rotb = b - b_
rota = a_ - a - rotb
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV, rotb))
# posture.rotate_global_orientation(joint_parent, mathlib.exp(rotV * rota))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV, rotb))
posture.mulJointOrientationGlobal(joint_parent, mm.exp(rotV * rota))
# rotate plane
# rotV2 = mathlib.normalize(numpy.cross(L, L_new))
# l_new = mathlib.length(L_new)
# l_diff = mathlib.length(L_new - L)
# rot2 = mathlib.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
# posture.rotate_global_orientation(joint_parent_parent, mathlib.exp(rotV2, rot2))
rotV2 = mm.normalize2(np.cross(L, L_new))
l_new = mm.length(L_new)
l_diff = mm.length(L_new - L)
rot2 = mm.ACOS((l_new * l_new + l * l - l_diff * l_diff) / (2 * l_new * l))
posture.mulJointOrientationGlobal(joint_parent_parent, mm.exp(rotV2, rot2))
return posture
def _renderJoint(self, joint, posture):
glPushMatrix()
glTranslatef(joint.offset[0], joint.offset[1], joint.offset[2])
# glMultMatrixf(mm.R2T(posture.localRMap[joint.name]).transpose())
glMultMatrixf(
mm.R2T(posture.localRs[posture.skeleton.getElementIndex(
joint.name)]).transpose())
# if joint.name in self.partColors:
# color = self.partColors[joint.name]
# else:
# color = self.totalColor
if joint == self.selectedElement:
glColor3ubv(SELECTION_COLOR)
ygh.beginDraw()
ygh.drawCoordinate()
ygh.endDraw()
# 1
# ygh.drawPoint((0,0,0), color)
if self.linkStyle == LINK_LINE:
self.rc.drawPoint((0, 0, 0))
for childJoint in joint.children:
self.rc.drawLine((0, 0, 0), childJoint.offset)
elif self.linkStyle == LINK_BONE:
# self.rc.drawPoint((0,0,0))
self.rc.drawLine((-.05, 0, 0), (.05, 0, 0))
for childJoint in joint.children:
self.rc.drawLine((0, 0, 0), childJoint.offset)
elif self.linkStyle == LINK_SOLIDBOX or self.linkStyle == LINK_WIREBOX:
if len(joint.children) > 0:
glPushMatrix()
offset = numpy.array([0., 0., 0.])
for childJoint in joint.children:
offset += childJoint.offset
offset = offset / len(joint.children)
defaultBoneV = numpy.array([0, 0, mm.length(offset)])
boneT = mm.R2T(mm.getSO3FromVectors(defaultBoneV, offset))
glMultMatrixf(boneT.transpose())
glTranslatef(-.05, -.05, 0)
# ygh.beginDraw()
# ygh.drawCoordinate()
# ygh.endDraw()
self.rc.drawBox(.1, .1, mm.length(offset))
glPopMatrix()
if joint == self.selectedElement:
glColor3ubv(self.totalColor)
for childJoint in joint.children:
self._renderJoint(childJoint, posture)
glPopMatrix()
def simulateCallback(frame):
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
motionModel.update(motion[frame])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam()
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
if Bsc == 0.0:
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1 + Bsc * 3)
viewer.motionViewWnd.update(1, viewer.doc)
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
linkPositions, allLinkJointMasks)
yjc.computeJacobian2(jFootL[0], DOFs, jointPositions, jointAxeses,
[positionFootL[0]], jointMasksFootL[0])
yjc.computeJacobianDerivative2(dJFootL[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootL[0]], jointMasksFootL[0],
False)
yjc.computeJacobian2(jFootR[0], DOFs, jointPositions, jointAxeses,
[positionFootR[0]], jointMasksFootR[0])
yjc.computeJacobianDerivative2(dJFootR[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootR[0]], jointMasksFootR[0],
False)
yjc.computeAngJacobian2(jAngFootL[0], DOFs, jointPositions,
jointAxeses, [positionFootL[0]],
jointMasksFootL[0])
yjc.computeAngJacobianDerivative2(dJAngFootL[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootL[0]],
jointMasksFootL[0], False)
yjc.computeAngJacobian2(jAngFootR[0], DOFs, jointPositions,
jointAxeses, [positionFootR[0]],
jointMasksFootR[0])
yjc.computeAngJacobianDerivative2(dJAngFootR[0], DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[positionFootR[0]],
jointMasksFootR[0], False)
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0)
##########################################
for i in range(len(rd_contactPositions)):
rd_contactPositions[i] = [0, 0, 0]
rd_contactForces[i] = [0, 0, 0]
for i in range(len(contactPositions)):
rd_contactPositions[i] = np.copy(contactPositions[i])
rd_contactForces[i] = np.copy(contactForces[i])
'''
if len(contactPositions) > 0:
rd_contactPositions = np.copy(contactPositions)
rd_contactForces = np.copy(contactForces)
print("rd_contactPositions", rd_contactPositions)
print("contactPositions", contactPositions)
'''
'''
for i in range(len(contactRendererName)):
viewer.doc.removeRenderer(contactRendererName[i])
del contactRendererName[:]
for i in range(len(contactPositions)):
contactRendererName.append(str(i))
#viewer.doc.addRenderer(str(i), yr.PointsRenderer([contactPositions[i]], (0,255,0)))
viewer.doc.addRenderer(str(i), yr.ForcesRenderer([contactForces[i]], [contactPositions[i]], (0,255,0), .009, 0.009))
viewer.motionViewWnd.update(1, viewer.doc)
'''
##########################################
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
if j != 0:
yjc.computePartialJacobian2(jFootL[j], DOFs,
jointPositions,
jointAxeses,
[positionFootL[j]],
jointMasksFootL[j],
partialDOFIndex)
yjc.computePartialJacobianDerivative2(
dJFootL[j], DOFs, jointPositions, jointAxeses,
linkAngVelocities, [positionFootL[j]],
jointMasksFootL[j], False, partialDOFIndex)
break
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
if j != 0:
yjc.computePartialJacobian2(jFootR[j], DOFs,
jointPositions,
jointAxeses,
[positionFootR[j]],
jointMasksFootR[j],
partialDOFIndex)
yjc.computePartialJacobianDerivative2(
dJFootR[j], DOFs, jointPositions, jointAxeses,
linkAngVelocities, [positionFootR[j]],
jointMasksFootR[j], False, partialDOFIndex)
break
for j in range(len(indexFootL)):
yjc.computeAngJacobian2(jAngFootL[j], DOFs, jointPositions,
jointAxeses, [positionFootL[j]],
jointMasksFootL[j])
yjc.computeAngJacobianDerivative2(dJAngFootL[j], DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[positionFootL[j]],
jointMasksFootL[j], False)
yjc.computeAngJacobian2(jAngFootR[j], DOFs, jointPositions,
jointAxeses, [positionFootR[j]],
jointMasksFootR[j])
yjc.computeAngJacobianDerivative2(dJAngFootR[j], DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[positionFootR[j]],
jointMasksFootR[j], False)
'''
if footPartNum == 1:
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
else:
if ((contactFlagFootL[3] == 1 or contactFlagFootL[4] == 1) and contactFlagFootL[0] == 0) or ((contactFlagFootR[3] == 1 or contactFlagFootR[4] == 1) and contactFlagFootR[0] == 0):
r = 0.8
footCenterL = (controlModel.getBodyPositionGlobal(supL)*r + controlModel.getBodyPositionGlobal(indexFootL[1])*(1.0-r))
footCenterR = (controlModel.getBodyPositionGlobal(supR)*r + controlModel.getBodyPositionGlobal(indexFootR[1])*(1.0-r))
#footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
#footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
else :
#footCenterL = (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(indexFootL[1]))/2.0
#footCenterR = (controlModel.getBodyPositionGlobal(supR) + controlModel.getBodyPositionGlobal(indexFootR[1]))/2.0
#footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
#footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
r = 0.8
footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1])*r + controlModel.getBodyPositionGlobal(indexFootL[3])*(1.0-r))
footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1])*r + controlModel.getBodyPositionGlobal(indexFootR[3])*(1.0-r))
'''
'''
if stage == POWERFUL_BALANCING:
footCenterL = controlModel.getBodyPositionGlobal(indexFootL[1])
footCenterR = controlModel.getBodyPositionGlobal(indexFootR[1])
else:
footCenterL = (controlModel.getBodyPositionGlobal(indexFootL[1]) + controlModel.getBodyPositionGlobal(indexFootL[3]) )/2.0
footCenterR = (controlModel.getBodyPositionGlobal(indexFootR[1]) + controlModel.getBodyPositionGlobal(indexFootR[3]))/2.0
'''
'''
p1 = controlModel.getBodyPositionGlobal(indexFootL[0])
p2 = controlModel.getBodyPositionGlobal(indexFootR[0])
p3 = controlModel.getBodyPositionGlobal(indexFootL[4])
p4 = controlModel.getBodyPositionGlobal(indexFootR[4])
print(frame, "supL", p1[1])
print(frame, "supR", p2[1])
print(frame, "calcL", p3[1])
print(frame, "calcR", p4[1])
'''
#footCenter = footCenterL + (footCenterR - footCenterL)/2.0
#footCenter[1] = 0.
#
'''
if checkAll(contactFlagFootL, 0) == 1 and checkAll(contactFlagFootR, 0) == 1:
footCenter = footCenter
elif checkAll(contactFlagFootL, 0) == 1 :
footCenter = footCenterR
elif checkAll(contactFlagFootR, 0) == 1 :
footCenter = footCenterL
'''
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = 1.
desFCL = (controlModel.getBodyPositionGlobal(supL) * r +
controlModel.getBodyPositionGlobal(indexFootL[2]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(supR) * r +
controlModel.getBodyPositionGlobal(indexFootR[2]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
if checkAll(contactFlagFootL, 0) == 1 and checkAll(
contactFlagFootR, 0) == 1:
desFC = desFC
elif checkAll(contactFlagFootL, 0) == 1:
desFC = desFCR
elif checkAll(contactFlagFootR, 0) == 1:
desFC = desFCL
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
vecRatio = mm.length(curRelCMVec) * 10.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
# linear momentum
CM_ref_plane = footCenter
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
# angular momentum
CP_ref = footCenter
timeStep = 30.
if CP_old[0] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / timeStep)
CP_old[0] = CP
if CP != None and dCP != None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
#dH_des = np.cross((CP_des - CM_plane), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
##############################
# soft point constraint
#####################################################
P_cur = controlModel.getBodyPositionGlobal(constBody)
constBodyVec = P_cur - footCenter
softConstPoint = [
footCenter[0] + softConstPointOffset[0],
mm.length(constBodyVec), footCenter[2] + softConstPointOffset[2]
]
#####################################################
P_des = softConstPoint
#P_cur = controlModel.getBodyPositionGlobal(constBody)
dP_des = [0, 0, 0]
dP_cur = controlModel.getBodyVelocityGlobal(constBody)
ddP_des1 = Ksc * (-(P_des - P_cur)) - Dsc * (-(dP_cur - dP_des))
r = -(P_des - P_cur)
I = np.vstack(([1, 0, 0], [0, 1, 0], [0, 0, 1]))
Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
yjc.computeJacobian2(Jconst, DOFs, jointPositions, jointAxeses,
[P_cur], constJointMasks)
JL, JA = np.vsplit(Jconst, 2)
Q1 = np.dot(Z, Jconst)
q1 = np.dot(JA, dth_flat)
q2 = np.dot(mm.getCrossMatrixForm(q1),
np.dot(mm.getCrossMatrixForm(q1), r))
yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions,
jointAxeses, linkAngVelocities, [P_cur],
constJointMasks, False)
q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
##############################
flagContact = True
if dH_des == None or np.any(np.isnan(dH_des)) == True:
flagContact = False
viewer.doc.showRenderer('rd_grf_des', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('rd_grf_des', True)
viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
#MOTION = FORWARD_JUMP
if mit.MOTION == mit.FORWARD_JUMP:
frame_index = [136, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
#frame_index = [100000, 100000]
elif mit.MOTION == mit.TAEKWONDO2:
frame_index = [130 + 40, 100]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
'''
if frame > 300 :
if stage != DYNAMIC_BALANCING:
print("#", frame,"-DYNAMIC_BALANCING")
stage = DYNAMIC_BALANCING
Kk = Kk*1
Dk = 2*(Kk**.5)
'''
if frame > frame_index[0]:
if stage != POWERFUL_BALANCING:
print("#", frame, "-POWERFUL_BALANCING")
stage = POWERFUL_BALANCING
Kk = Kk * 2
Dk = 2 * (Kk**.5)
elif frame > frame_index[1]:
if stage != MOTION_TRACKING:
print("#", frame, "-MOTION_TRACKING")
stage = MOTION_TRACKING
trackingW = w
if stage == MOTION_TRACKING:
trackingW = w2
Bt = Bt * 2
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1,
q_bias1)
if flagContact == True:
if stage != MOTION_TRACKING:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R,
r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = [None]
Jsup_2 = [None]
dJsup_2 = [None]
##############################
# Hard constraint
if stage != MOTION_TRACKING:
Kk2 = Kk * 2.0
else:
Kk2 = Kk * 1.5
Dk2 = 2 * (Kk2**.5)
'''
desLinearAccL, desPosL = getDesFootLinearAcc(motionModel, controlModel, supL, ModelOffset, CM_ref, CM, Kk2, Dk2)
desLinearAccR, desPosR = getDesFootLinearAcc(motionModel, controlModel, supR, ModelOffset, CM_ref, CM, Kk2, Dk2)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL, Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR, Kk2, Dk2)
'''
if stage != MOTION_TRACKING:
idx = 0 #LEFT/RIGHT_TOES
desLinearAccL, desPosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[idx], ModelOffset,
CM_ref, CM, Kk2, Dk2, 0.040)
desLinearAccR, desPosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[idx], ModelOffset,
CM_ref, CM, Kk2, Dk2, 0.040)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel,
indexFootL[idx], Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel,
indexFootR[idx], Kk2, Dk2)
a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)),
np.hstack((desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((jFootL[idx], jFootR[idx]))
dJsup_2 = np.vstack((dJFootL[idx], dJFootR[idx]))
'''
idx = 0
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[idx], Kk2, Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[idx], Kk2, Dk2)
a_sup_2 = np.hstack(( desAngularAccL, desAngularAccR))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
'''
rd_DesPosL[0] = desPosL.copy()
rd_DesPosR[0] = desPosR.copy()
else:
if footPartNum == 5:
idx = 3
desAngularAccL = getDesFootAngularAcc(motionModel,
controlModel,
indexFootL[idx], Kk2,
Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel,
controlModel,
indexFootR[idx], Kk2,
Dk2)
a_sup_2 = np.hstack((desAngularAccL, desAngularAccR))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
idx = 4
desAngularAccL = getDesFootAngularAcc(motionModel,
controlModel,
indexFootL[idx], Kk2,
Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel,
controlModel,
indexFootR[idx], Kk2,
Dk2)
a_sup_2 = np.hstack(
(a_sup_2, np.hstack((desAngularAccL, desAngularAccR))))
Jsup_2 = np.vstack(
(Jsup_2, np.vstack((jAngFootL[idx], jAngFootR[idx]))))
dJsup_2 = np.vstack(
(dJsup_2, np.vstack((dJAngFootL[idx], dJAngFootR[idx]))))
else:
idx = 1
desAngularAccL = getDesFootAngularAcc(motionModel,
controlModel,
indexFootL[idx], Kk2,
Dk2)
desAngularAccR = getDesFootAngularAcc(motionModel,
controlModel,
indexFootR[idx], Kk2,
Dk2)
a_sup_2 = np.hstack((desAngularAccL, desAngularAccR))
Jsup_2 = np.vstack((jAngFootL[idx], jAngFootR[idx]))
dJsup_2 = np.vstack((dJAngFootL[idx], dJAngFootR[idx]))
##############################
##############################
# Additional constraint
if stage != MOTION_TRACKING:
#Kk2 = Kk * 2.5
Kk2 = Kk * 2.5
Dk2 = 2 * (Kk2**.5)
desForePosL = [0, 0, 0]
desForePosR = [0, 0, 0]
desRearPosL = [0, 0, 0]
desRearPosR = [0, 0, 0]
for i in range(1, footPartNum):
if i == i:
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(
motionModel, controlModel, indexFootL[i],
ModelOffset, CM_ref, CM, Kk2, Dk2, 0.04)
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack(
(a_sup_2, np.hstack(
(desLinearAccL, desAngularAccL))))
Jsup_2 = np.vstack((Jsup_2, jFootL[i]))
dJsup_2 = np.vstack((dJsup_2, dJFootL[i]))
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(
motionModel, controlModel, indexFootR[i],
ModelOffset, CM_ref, CM, Kk2, Dk2, 0.04)
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack(
(a_sup_2, np.hstack(
(desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((Jsup_2, jFootR[i]))
dJsup_2 = np.vstack((dJsup_2, dJFootR[i]))
else:
if contactFlagFootL[i] == 1:
desAngularAccL = getDesFootAngularAcc(
motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack((a_sup_2, desAngularAccL))
Jsup_2 = np.vstack((Jsup_2, jAngFootL[i]))
dJsup_2 = np.vstack((dJsup_2, dJAngFootL[i]))
if contactFlagFootR[i] == 1:
desAngularAccR = getDesFootAngularAcc(
motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack((a_sup_2, desAngularAccR))
Jsup_2 = np.vstack((Jsup_2, jAngFootR[i]))
dJsup_2 = np.vstack((dJsup_2, dJAngFootR[i]))
'''
for i in range(1, footPartNum) :
if contactFlagFootL[i] == 1:
desLinearAccL, desForePosL = getDesFootLinearAcc(motionModel, controlModel, indexFootL[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, indexFootL[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccL, desAngularAccL)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootL[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootL[i] ))
if contactFlagFootR[i] == 1:
desLinearAccR, desForePosR = getDesFootLinearAcc(motionModel, controlModel, indexFootR[i], ModelOffset, CM_ref, CM, Kk2, Dk2, 0.034)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, indexFootR[i], Kk2, Dk2)
a_sup_2 = np.hstack(( a_sup_2, np.hstack((desLinearAccR, desAngularAccR)) ))
Jsup_2 = np.vstack(( Jsup_2, jFootR[i] ))
dJsup_2 = np.vstack(( dJsup_2, dJFootR[i] ))
'''
rd_DesForePosL[0] = desForePosL
rd_DesForePosR[0] = desForePosR
rd_DesRearPosL[0] = desRearPosL
rd_DesRearPosR[0] = desRearPosR
##############################
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat,
a_sup_2)
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
'''
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0 or extraForce[0][2] != 0) :
forceApplyFrame += 1
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame*wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
'''
vpWorld.step()
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - footCenter) * 3
#rd_CM_plane[0][1] = 0.
if CP != None and dCP != None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
if (forceApplyFrame == 0):
applyedExtraForce[0] = [0, 0, 0]
def PDControl(frame):
# global deg1[0], J1, J2, M1, M2
# scalar Kp = 1000.;
# scalar Kd = 100.;
Kp = 100.
Kd = 2.
# SE3 desiredOri1 = Exp(Axis(axis1), scalar(deg1[0] * M_RADIAN));
# SE3 desiredOri2 = Exp(Axis(axis2), scalar(deg2 * M_RADIAN));
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
# desiredOri2 = mm.exp(axis2, deg2 * M_RADIAN)
# se3 log1= Log(J1.GetOrientation() % desiredOri1);
# se3 log2= Log(J2.GetOrientation() % desiredOri2);
parent1 = J1.getBody(0)
child1 = J1.getBody(1)
parent1_desired_SO3 = mm.exp((0, 0, 0), 0)
child1_desired_SO3 = desiredOri1
# child1_desired_SO3 = parent1_desired_SO3
parent1_body_SO3 = mm.odeSO3ToSO3(parent1.getRotation())
child1_body_SO3 = mm.odeSO3ToSO3(child1.getRotation())
# init_ori = (mm.exp((1,0,0),math.pi/2))
# child1_body_SO3 = numpy.dot(mm.odeSO3ToSO3(child1.getRotation()), init_ori.transpose())
align_SO3 = numpy.dot(parent1_body_SO3,
parent1_desired_SO3.transpose())
child1_desired_SO3 = numpy.dot(align_SO3, child1_desired_SO3)
diff_rot = mm.logSO3(
numpy.dot(child1_desired_SO3, child1_body_SO3.transpose()))
# print diff_rot
parent_angleRate = parent1.getAngularVel()
child_angleRate = child1.getAngularVel()
# print child_angleRate
angleRate = numpy.array([
-parent_angleRate[0] + child_angleRate[0],
-parent_angleRate[1] + child_angleRate[1],
-parent_angleRate[2] + child_angleRate[2]
])
# torque1 = Kp*diff_rot - Kd*angleRate
# print torque1
# J1_ori =
# log1 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# log2 = mm.logSO3_tuple(numpy.dot(desiredOri1 ,J1.GetOrientation().transpose()))
# Vec3 torque1 = Kp*(Vec3(log1[0],log1[1],log1[2])) - Kd*J1.GetVelocity();
# Vec3 torque2 = Kp*(Vec3(log2[0],log2[1],log2[2])) - Kd*J2.GetVelocity();
M1.setAxis(0, 0, diff_rot)
M1.setAxis(1, 0, angleRate)
# M2.setAxis(0,0,torque1)
## J1.SetTorque(torque1);
## J2.SetTorque(torque2);
M1.addTorques(-Kp * mm.length(diff_rot), 0, 0)
M1.addTorques(0, Kd * mm.length(angleRate), 0)
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
motionModel.recordVelByFiniteDiff()
motionOriModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
footPartNum = config['FootPartNum']
if footPartNum > 1:
elasticity = 2000
damping = 2 * (elasticity**.5)
springBody1 = 5
springBody2 = 6
springBody1Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos) * 1.
node = mcfg.getNode(mit.LEFT_PHALANGE_1)
initialDist -= node.width #0.084
v1 = (-node.width * 0.5, 0.0, node.length * 0.4)
v2 = (node.width * 0.5, 0.0, node.length * 0.4)
controlModel.setSpring(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]),
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]),
elasticity, damping, v2, v1, initialDist)
controlModel.setSpring(
motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]),
motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]),
elasticity, damping, v1, v2, initialDist)
#elasticity = 10
#damping = 2*(elasticity**.5)
#springBody1 = 3
#springBody2 = 4
#node = mcfg.getNode(mit.LEFT_PHALANGE_1)
#springBody1Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
#springBody2Pos = motionModel.getBodyPositionGlobal(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
#initialDist = mm.length(springBody1Pos - springBody2Pos)*1.
#initialDist -= node.width#0.084
#v1 = (-node.width*0.5,0.0,-node.length*0.4)
#v2 = (node.width*0.5,0.0,-node.length*0.4)
##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
##controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#ModelOffset = (1.5, -0.01, 0)
ModelOffset = (1.5, 0.04, 0)
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2'])
#w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
rootB = motion[0].skeleton.getJointIndex(config['root'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex(config['const'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys_IK = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
Jcom = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcom = Jcom.copy()
JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcomAng = JcomAng.copy()
###############
jFootL_IK = [None] * footPartNum
jFootR_IK = [None] * footPartNum
indexFootL = [None] * footPartNum
indexFootR = [None] * footPartNum
jFootL = [None] * footPartNum
dJFootL = [None] * footPartNum
jFootR = [None] * footPartNum
dJFootR = [None] * footPartNum
jointMasksFootL = [None] * footPartNum
jointMasksFootR = [None] * footPartNum
for i in range(footPartNum):
jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootL[i] = jFootL[i].copy()
jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootR[i] = jFootR[i].copy()
indexFootL[i] = motion[0].skeleton.getJointIndex(
config['FootLPart'][i])
indexFootR[i] = motion[0].skeleton.getJointIndex(
config['FootRPart'][i])
jointMasksFootL[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i])
]
jointMasksFootR[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i])
]
constJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton,
[indexFootL[0], indexFootR[0]])
]
#constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])]
#constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks = [
yjc.getLinkJointMask(motion[0].skeleton, selectedBody)
]
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks[0][0] = 0
#comUpperJointMasks[0][1] = 1
#comUpperJointMasks[0][10] = 1
comUpperJointMasks[0][2] = 1
comUpperJointMasks[0][11] = 1
#print(comUpperJointMasks)
comLowerJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton, [
motion[0].skeleton.getJointIndex('LeftLeg'),
motion[0].skeleton.getJointIndex('RightLeg')
])
]
'''
maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks]
parentArray = [supL, supR, supL, supR]
effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR]
for j in range(4) :
for i in range(len(foreSupLJointMasks)) :
if i == parentArray[j] or i == effectorArray[j] :
maskArray[j][0][i] = 1
else :
maskArray[j][0][i] = 0
'''
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
qps = hqp.QPSimulator()
problem = yac.LSE(totalDOF, 6)
a_sup = (0, 0, 0, 0, 0, 0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0, 0, 0, 0, 0, 0] #R
a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
dth_IK = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM_ref = [None]
rd_CM_des = [None]
rd_CM = [None]
rd_CM_vec = [None]
rd_CM_ref_vec = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_footCenter_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_root = [None]
rd_footL_vec = [None]
rd_footR_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rd_DesForePosL = [None]
rd_DesForePosR = [None]
rd_DesRearPosL = [None]
rd_DesRearPosR = [None]
rd_Joint = [None]
rd_Joint2 = [None]
rd_Joint3 = [None]
rd_Joint4 = [None]
rd_desPoints = [None]
#rd_contactForces = [None]*10000
#rd_contactPositions = [None]*10000
rd_virtualForce = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0, 0, 0]
normalVector = [[0, 2, 0]]
if MULTI_VIEWER:
viewer = ymv.MultiViewer(800, 655)
#viewer = ymv.MultiViewer(1600, 1255)
viewer.setRenderers1([
cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
viewer.setRenderers2([
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
else:
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (100, 100, 100),
yr.POLYGON_FILL)) #(150,150,255)
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR,
yr.POLYGON_FILL))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_LINE))
#viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
#viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) )
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CM',
yr.PointsRenderer(rd_CM, (255, 0, 255)))
viewer.doc.addRenderer('rd_CM_des',
yr.PointsRenderer(rd_CM_des, (64, 64, 255)))
viewer.doc.addRenderer(
'rd_CM_vec',
yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 128)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001))
viewer.doc.addRenderer(
'rd_exf_des',
yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009,
0.04))
#viewer.doc.addRenderer('rd_CMP', yr.PointsRenderer(rd_CMP, (0,0,255)))
#viewer.doc.addRenderer('rd_DesPosL', yr.PointsRenderer(rd_DesPosL, (0,0,255)))
#viewer.doc.addRenderer('rd_DesPosR', yr.PointsRenderer(rd_DesPosR, (0,100,255)))
#viewer.doc.addRenderer('rd_DesForePosL', yr.PointsRenderer(rd_DesForePosL, (150,0,200)))
#viewer.doc.addRenderer('rd_DesForePosR', yr.PointsRenderer(rd_DesForePosR, (150,0,250)))
#viewer.doc.addRenderer('rd_DesRearPosL', yr.PointsRenderer(rd_DesRearPosL, (0,150,200)))
#viewer.doc.addRenderer('rd_DesRearPosR', yr.PointsRenderer(rd_DesRearPosR, (0,150,250)))
#viewer.doc.addRenderer('softConstraint', yr.VectorsRenderer(rd_soft_const_vec, rd_CMP, (150,100,100), 3))
#viewer.doc.addRenderer('rd_footLVec', yr.VectorsRenderer(rd_footL_vec, rd_footCenterL, (255,0,0), 3))
#viewer.doc.addRenderer('rd_footRVec', yr.VectorsRenderer(rd_footR_vec, rd_footCenterR, (255,255,0), 3))
#viewer.doc.addRenderer('rd_footCenter_ref', yr.PointsRenderer(rd_footCenter_ref))
#viewer.doc.addRenderer('rd_CM_plane_ref', yr.PointsRenderer(rd_CM_plane_ref, (255,255,0)))
#viewer.doc.addRenderer('rd_refNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter_ref, (255,0,0), 3))
#viewer.doc.addRenderer('rd_refCMVec', yr.VectorsRenderer(rd_CM_ref_vec, rd_footCenter_ref, (255,0,255), 3))
#viewer.doc.addRenderer('rd_curNormalVec', yr.VectorsRenderer(normalVector, rd_footCenter, (255,0,0), 3))
#viewer.doc.addRenderer('rd_CMVec', yr.VectorsRenderer(rd_CM_vec, rd_footCenter, (255,0,255), 3))
#viewer.doc.addRenderer('rd_contactForces', yr.ForcesRenderer(rd_contactForces, rd_contactPositions, (0,255,0), .009, 0.009))
#viewer.doc.addRenderer('rd_virtualForce', yr.ForcesRenderer(rd_virtualForce, rd_CM, (50,255,0), 0.5, 0.02))
#viewer.doc.addRenderer('rd_Joint', yr.PointsRenderer(rd_Joint, (255,0,0)))
#viewer.doc.addRenderer('rd_Joint2', yr.PointsRenderer(rd_Joint2, (0,255,0)))
#viewer.doc.addRenderer('rd_Joint3', yr.PointsRenderer(rd_Joint3, (0,0,255)))
#viewer.doc.addRenderer('rd_Joint4', yr.PointsRenderer(rd_Joint4, (255,255,0)))
viewer.doc.addRenderer('rd_desPoints',
yr.PointsRenderer(rd_desPoints, (255, 0, 0)))
stage = STATIC_BALANCING
contactRendererName = []
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
desCOMOffset = 0.0
pt = [0.]
timeReport = [0.] * 7
viewer.objectInfoWnd.comOffsetY.value(-0.05)
viewer.objectInfoWnd.comOffsetZ.value(0.00)
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc')
viewer.objectInfoWnd.Bc.value(0.1)
viewer.objectInfoWnd.ankleAngleX = Fl_Value_Input(50, 510, 40, 10,
'Ankle X')
viewer.objectInfoWnd.ankleAngleX.value(0)
viewer.objectInfoWnd.ankleAngleY = Fl_Value_Input(110, 510, 40, 10, 'Y')
viewer.objectInfoWnd.ankleAngleY.value(1)
viewer.objectInfoWnd.ankleAngleZ = Fl_Value_Input(170, 510, 40, 10, 'Z')
viewer.objectInfoWnd.ankleAngleZ.value(0)
viewer.objectInfoWnd.end()
viewer.objectInfoWnd.labelKt.value(50)
viewer.objectInfoWnd.labelKk.value(17)
config['Phalange'] = [ motion[0].skeleton.getJointIndex('LeftPhalange_1'),\
motion[0].skeleton.getJointIndex('LeftPhalange_2'),\
motion[0].skeleton.getJointIndex('RightPhalange_1'),\
motion[0].skeleton.getJointIndex('RightPhalange_2')]
config['Talus'] = [ motion[0].skeleton.getJointIndex('LeftTalus_1'),\
motion[0].skeleton.getJointIndex('LeftTalus_2'),\
motion[0].skeleton.getJointIndex('RightTalus_1'),\
motion[0].skeleton.getJointIndex('RightTalus_2')]
config['Calcaneus'] = [ motion[0].skeleton.getJointIndex('LeftCalcaneus_1'),\
motion[0].skeleton.getJointIndex('LeftCalcaneus_2'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_1'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_2')]
pose = motion[0].copy()
timeReport = [0.] * 2
def simulateCallback(frame):
curTime = time.time()
Ke = 0.0
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
motionModel.update(motion[frame])
controlToMotionOffset = [-2.0, 0., 0.]
motionModel.translateByOffset(controlToMotionOffset)
stepsPerFrame = 10
for i in range(stepsPerFrame):
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
#Kt, Kl, Kh, Bl, Bh, Ke = viewer.GetParam()
#qps.setupWeight(Kt, Kl, Kh, Ke, Bt, Btau, Bcon, Bl, Bh, Be)
qps.setupWeight(Kt, Kl, Kh, Ke, 10., .1, .1, Bl, Bh, 10.)
cPositions, CP, CM, footCenter, dL_des, CM_ref = qps.setupQP(
frame, motion, mcfg, controlModel, vpWorld, config,
1. / (30. * stepsPerFrame))
CM_ref[1] = 0.
timeReport[0] += time.time() - curTime
curTime = time.time()
#forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
#extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce)
#extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
#if viewer.GetForceState() :
# qps.addExternalForces(extraForce[0], selectedBody, viewer.objectInfoWnd.labelForceDur.value());
# viewer.ResetForceState()
x, cForce = qps.stepQP(controlModel, 1. / (30. * stepsPerFrame))
timeReport[1] += time.time() - curTime
curTime = time.time()
print timeReport
if frame % 30 == 0:
print 'elapsed time for 30 frames:', time.time() - pt[0]
# rendering
#rd_footCenter[0] = footCenter
#
#rd_CM[0] = CM.copy()
#
#rd_CM_plane[0] = CM_plane.copy()
#
#rd_footCenter_ref[0] = footCenter_ref
#rd_CM_plane_ref[0] = CM_ref.copy()
#rd_CM_ref[0] = CM_ref.copy()
#rd_CM_ref_vec[0] = (CM_ref - footCenter_ref)*3.
#rd_CM_vec[0] = (CM - CM_plane)
#rd_CM_des[0] = CM_ref_plane.copy()
#rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
#if CP!=None and dCP!=None:
# # rd_CP[0] = CP
# # rd_CP_des[0] = CP_des
# #
# #rd_dL_des_plane[0] = dL_des_plane
# #rd_dH_des[0] = dH_des
# #
# #rd_grf_des[0] = totalNormalForce# - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
# #
# #rd_exf_des[0] = applyedExtraForce[0]
# #rd_root_des[0] = rootPos[0]
#
# #rd_CMP[0] = softConstPoint
#
# #rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint
#
#
# ##indexL = motion[0].skeleton.getJointIndex('Hips')
# ##indexR = motion[0].skeleton.getJointIndex('Spine1')
# #indexL = indexFootL[0]
# #indexR = indexFootR[0]
#
# #curAng = [controlModel.getBodyOrientationGlobal(indexL)]
# #curAngY = np.dot(curAng, np.array([0,0,1]))
#
# #rd_footL_vec[0] = np.copy(curAngY[0])
# #rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL)
# #
# #curAng = [controlModel.getBodyOrientationGlobal(indexR)]
# #curAngY = np.dot(curAng, np.array([0,0,1]))
#
#rd_footR_vec[0] = np.copy(curAngY[0])
#rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR)
#
#if (forceApplyFrame == 0) :
# applyedExtraForce[0] = [0, 0, 0]
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
motionModel.recordVelByFiniteDiff()
IKModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
footPartNum = config['FootPartNum']
if footPartNum > 1:
elasticity = 2000
damping = 2 * (elasticity**.5)
springBody1 = 1
springBody2 = 2
springBody1Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos) * 1.
node = mcfg.getNode(mit.LEFT_METATARSAL_1)
initialDist -= node.width #0.084
v1 = (-node.width * 0.5, 0.0, node.length * 0.4)
v2 = (node.width * 0.5, 0.0, node.length * 0.4)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
elasticity = 10
damping = 2 * (elasticity**.5)
springBody1 = 3
springBody2 = 4
node = mcfg.getNode(mit.LEFT_PHALANGE_1)
springBody1Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]))
springBody2Pos = motionModel.getBodyPositionGlobal(
motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]))
initialDist = mm.length(springBody1Pos - springBody2Pos) * 1.
initialDist -= node.width #0.084
v1 = (-node.width * 0.5, 0.0, -node.length * 0.4)
v2 = (node.width * 0.5, 0.0, -node.length * 0.4)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootLPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootLPart'][springBody2]), elasticity, damping, v2, v1, initialDist)
#controlModel.setSpring(motion[0].skeleton.getJointIndex(config['FootRPart'][springBody1]), motion[0].skeleton.getJointIndex(config['FootRPart'][springBody2]), elasticity, damping, v1, v2, initialDist)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#ModelOffset = (1.5, -0.01, 0)
ModelOffset = (1.5, 0.04, 0)
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
w2 = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap2'])
#w_IK = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['IKweightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
rootB = motion[0].skeleton.getJointIndex(config['root'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
#constBody = motion[0].skeleton.getJointIndex('LeftForeArm')
constBody = motion[0].skeleton.getJointIndex(config['const'])
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
JsupPre = Jsup.copy()
Jsys_IK = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
Jcom = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcom = Jcom.copy()
JcomAng = yjc.makeEmptyJacobian(DOFs, 1, False)
dJcomAng = JcomAng.copy()
###############
jFootL_IK = [None] * footPartNum
jFootR_IK = [None] * footPartNum
indexFootL = [None] * footPartNum
indexFootR = [None] * footPartNum
jFootL = [None] * footPartNum
dJFootL = [None] * footPartNum
jFootR = [None] * footPartNum
dJFootR = [None] * footPartNum
jointMasksFootL = [None] * footPartNum
jointMasksFootR = [None] * footPartNum
jAngFootL = [None] * footPartNum
dJAngFootL = [None] * footPartNum
jAngFootR = [None] * footPartNum
dJAngFootR = [None] * footPartNum
for i in range(footPartNum):
jFootL_IK[i] = yjc.makeEmptyJacobian(DOFs, 1)
jFootR_IK[i] = yjc.makeEmptyJacobian(DOFs, 1)
jFootL[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootL[i] = jFootL[i].copy()
jFootR[i] = yjc.makeEmptyJacobian(DOFs, 1)
dJFootR[i] = jFootR[i].copy()
jAngFootL[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootL[i] = jAngFootL[i].copy()
jAngFootR[i] = yjc.makeEmptyJacobian(DOFs, 1, False)
dJAngFootR[i] = jAngFootR[i].copy()
indexFootL[i] = motion[0].skeleton.getJointIndex(
config['FootLPart'][i])
indexFootR[i] = motion[0].skeleton.getJointIndex(
config['FootRPart'][i])
jointMasksFootL[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootL[i])
]
jointMasksFootR[i] = [
yjc.getLinkJointMask(motion[0].skeleton, indexFootR[i])
]
constJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton,
[indexFootL[0], indexFootR[0]])
]
#constJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [indexFootL[0]])]
#constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks = [
yjc.getLinkJointMask(motion[0].skeleton, selectedBody)
]
#comLowerJointMasks = [yjc.getLinksJointMask(motion[0].skeleton, [motion[0].skeleton.getJointIndex('LeftLeg'), motion[0].skeleton.getJointIndex('RightLeg')])]
comUpperJointMasks[0][0] = 0
#comUpperJointMasks[0][1] = 1
#comUpperJointMasks[0][10] = 1
comUpperJointMasks[0][2] = 1
comUpperJointMasks[0][11] = 1
#print(comUpperJointMasks)
comLowerJointMasks = [
yjc.getLinksJointMask(motion[0].skeleton, [
motion[0].skeleton.getJointIndex('LeftLeg'),
motion[0].skeleton.getJointIndex('RightLeg')
])
]
'''
maskArray = [foreSupLJointMasks, foreSupRJointMasks, rearSupLJointMasks, rearSupRJointMasks]
parentArray = [supL, supR, supL, supR]
effectorArray = [foreSupL, foreSupR, rearSupL, rearSupR]
for j in range(4) :
for i in range(len(foreSupLJointMasks)) :
if i == parentArray[j] or i == effectorArray[j] :
maskArray[j][0][i] = 1
else :
maskArray[j][0][i] = 0
'''
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 6)
a_sup = (0, 0, 0, 0, 0, 0) #L
#a_sup2 = (0,0,0, 0,0,0)#R
a_sup2 = [0, 0, 0, 0, 0, 0] #R
a_sup_2 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
dth_IK = ype.makeNestedList(DOFs)
d_th_IK = ype.makeNestedList(DOFs)
d_th_IK_L = ype.makeNestedList(DOFs)
d_th_IK_R = ype.makeNestedList(DOFs)
dd_th_IK = ype.makeNestedList(DOFs)
dd_th_IK_flat = ype.makeFlatList(totalDOF)
d_th_IK_flat = ype.makeFlatList(totalDOF)
ddth_c_flat = ype.makeFlatList(totalDOF)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM_plane_ref = [None]
rd_CM_ref = [None]
rd_CM_des = [None]
rd_CM = [None]
rd_CM_vec = [None]
rd_CM_ref_vec = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_footCenter_des = [None]
rd_exf_des = [None]
rd_root_des = [None]
rd_soft_const_vec = [None]
rd_root = [None]
rd_footL_vec = [None]
rd_footR_vec = [None]
rd_CMP = [None]
rd_DesPosL = [None]
rd_DesPosR = [None]
rd_DesForePosL = [None]
rd_DesForePosR = [None]
rd_DesRearPosL = [None]
rd_DesRearPosR = [None]
rd_Joint = [None]
rd_Joint2 = [None]
rd_Joint3 = [None]
rd_Joint4 = [None]
#rd_contactForces = [None]*10000
#rd_contactPositions = [None]*10000
rd_virtualForce = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
applyedExtraForce = [None]
applyedExtraForce[0] = [0, 0, 0]
normalVector = [[0, 2, 0]]
if MULTI_VIEWER:
viewer = ymv.MultiViewer(800, 655)
#viewer = ymv.MultiViewer(1600, 1255)
viewer.setRenderers1([
cvr.VpModelRenderer(motionModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
viewer.setRenderers2([
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL)
])
else:
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
cvr.VpModelRenderer(motionModel, (100, 100, 100),
yr.POLYGON_FILL)) #(150,150,255)
viewer.doc.addRenderer(
'IKModel',
cvr.VpModelRenderer(IKModel, (180, 180, 180), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR,
yr.POLYGON_FILL))
#viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
#viewer.doc.addRenderer('rd_footCenter_des', yr.PointsRenderer(rd_footCenter_des, (150,0,150)) )
#viewer.doc.addRenderer('rd_footCenterL', yr.PointsRenderer(rd_footCenterL))
#viewer.doc.addRenderer('rd_footCenterR', yr.PointsRenderer(rd_footCenterR))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CM',
yr.PointsRenderer(rd_CM, (255, 0, 255)))
viewer.doc.addRenderer('rd_CM_des',
yr.PointsRenderer(rd_CM_des, (64, 64, 255)))
viewer.doc.addRenderer(
'rd_CM_vec',
yr.VectorsRenderer(rd_CM_vec, rd_CM_plane, (255, 0, 0), 3))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (0,255,0)))
#viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
# viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
# viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP, (0,255,255), .001))
viewer.doc.addRenderer(
'rd_exf_des',
yr.ForcesRenderer(rd_exf_des, rd_root_des, (0, 255, 0), .009,
0.04))
viewer.doc.addRenderer('rd_Joint',
yr.PointsRenderer(rd_Joint, (255, 0, 0)))
viewer.doc.addRenderer('rd_Joint2',
yr.PointsRenderer(rd_Joint2, (0, 255, 0)))
viewer.doc.addRenderer('rd_Joint3',
yr.PointsRenderer(rd_Joint3, (0, 0, 255)))
viewer.doc.addRenderer('rd_Joint4',
yr.PointsRenderer(rd_Joint4, (255, 255, 0)))
stage = STATIC_BALANCING
contactRendererName = []
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
desCOMOffset = 0.0
pt = [0.]
timeReport = [0.] * 7
viewer.objectInfoWnd.comOffsetY.value(-0.05)
viewer.objectInfoWnd.comOffsetZ.value(0.00)
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.Bc = Fl_Value_Input(100, 450, 40, 10, 'Bc')
viewer.objectInfoWnd.Bc.value(0.1)
viewer.objectInfoWnd.end()
viewer.objectInfoWnd.labelKt.value(50)
viewer.objectInfoWnd.labelKk.value(17)
config['Phalange'] = [ motion[0].skeleton.getJointIndex('LeftPhalange_1'),\
motion[0].skeleton.getJointIndex('LeftPhalange_2'),\
motion[0].skeleton.getJointIndex('LeftPhalange_3'),\
motion[0].skeleton.getJointIndex('RightPhalange_1'),\
motion[0].skeleton.getJointIndex('RightPhalange_2'),\
motion[0].skeleton.getJointIndex('RightPhalange_3')]
config['Metatarsal'] = [motion[0].skeleton.getJointIndex('LeftMetatarsal_1'),\
motion[0].skeleton.getJointIndex('LeftMetatarsal_2'),\
motion[0].skeleton.getJointIndex('LeftMetatarsal_3'),\
motion[0].skeleton.getJointIndex('RightMetatarsal_1'),\
motion[0].skeleton.getJointIndex('RightMetatarsal_2'),\
motion[0].skeleton.getJointIndex('RightMetatarsal_3')]
config['Talus'] = [ motion[0].skeleton.getJointIndex('LeftTalus_1'),\
motion[0].skeleton.getJointIndex('LeftTalus_2'),\
motion[0].skeleton.getJointIndex('LeftTalus_3'),\
motion[0].skeleton.getJointIndex('RightTalus_1'),\
motion[0].skeleton.getJointIndex('RightTalus_2'),\
motion[0].skeleton.getJointIndex('RightTalus_3')]
config['Calcaneus'] = [ motion[0].skeleton.getJointIndex('LeftCalcaneus_1'),\
motion[0].skeleton.getJointIndex('LeftCalcaneus_2'),\
motion[0].skeleton.getJointIndex('LeftCalcaneus_3'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_1'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_2'),\
motion[0].skeleton.getJointIndex('RightCalcaneus_3')]
def simulateCallback(frame):
curTime = time.time()
if frame % 30 == 1: pt[0] = time.time()
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
global desCOMOffset
#motionModel.update(motion[0])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
'''
if Bsc == 0.0 :
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1+Bsc*3)
viewer.motionViewWnd.update(1, viewer.doc)
'''
pose = motion[0].copy()
def solveIK(desComPos,
desIdxs,
desPos,
desOri,
cmW=10.,
posW=1.,
oriW=1.):
numItr = 100
dt = .5
threshold = 0.1
for i in range(0, numItr):
jPart_IK = []
print '----iter num', i
IKModel.update(pose)
th_r_IK = pose.getDOFPositions()
jointPositions_IK = pose.getJointPositionsGlobal()
jointAxeses_IK = pose.getDOFAxeses()
linkPositions_IK = IKModel.getBodyPositionsGlobal()
linkInertias_IK = IKModel.getBodyInertiasGlobal()
CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass)
print CM_IK
P_IK = ymt.getPureInertiaMatrix(TO, linkMasses,
linkPositions_IK, CM_IK,
linkInertias_IK)
yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK,
jointAxeses_IK, linkPositions_IK,
allLinkJointMasks)
for j in range(0, len(desIdxs)):
jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6])
J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2)
dv_IK = cmW * (desComPos - CM_IK)
for j in range(0, len(desIdxs)):
J_IK = np.vstack((J_IK, jPart_IK[j]))
pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j])
dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK))
ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j])
dv_IK = np.append(dv_IK,
oriW * mm.logSO3(desOri[j] * ori_IK.T))
#print dv_IK[0:3]
dth_IK_solve = npl.lstsq(J_IK, dv_IK)
dth_IK_x = dth_IK_solve[0][:totalDOF]
ype.nested(dth_IK_x, dth_IK)
#print dth_IK[0][0:3]
th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt)
pose.setDOFPositions(th_IK)
if np.dot(dv_IK, dv_IK) < threshold:
break
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
#CM_IK_ref = footCenter + footCenterOffset
CM_IK_ref = CM_ref + footCenterOffset
#CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1]
motion[0].skeleton.getJointIndex(config['supLink'])
#IKidxs = [indexFootL[0], indexFootR[0]]
#IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])]
#for i in range(0, 2):
# #IKdesPos[i] += ModelOffset
# IKdesPos[i][1] = 0.069
#IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])]
#IKdesOri = [None]*2
#for i in range(0, 2):
# IKdesOri[i] = mm.I_SO3()
IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4]
print IKidxs
IKdesPos = [None] * len(IKidxs)
IKdesOri = [None] * len(IKidxs)
for i in range(0, len(IKidxs)):
#print i
IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i])
IKdesPos[i][1] = 0.03
IKdesOri[i] = mm.I_SO3()
print IKdesPos
solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.))
#viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.))
#B_Hipd = viewer.objectInfoWnd.labelLeftHip.value()
#newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.)
#idx = motion[0].skeleton.getJointIndex('LeftUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
#idx = motion[0].skeleton.getJointIndex('RightUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
leftHipTimer -= 1
timeReport[0] += time.time() - curTime
curTime = time.time()
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
linkInertias_ref = motionModel.getBodyInertiasGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
timeReport[1] += time.time() - curTime
curTime = time.time()
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
timeReport[2] += time.time() - curTime
curTime = time.time()
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if frame > 0:
dJsys = (Jsys - JsysPre) * 30.
else:
dJsys = (Jsys - Jsys)
JsysPre = Jsys.copy()
timeReport[3] += time.time() - curTime
curTime = time.time()
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
for j in range(0, footPartNum):
jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy()
jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy()
dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6] #.copy()
dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6] #.copy()
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = .5 + desCOMOffset
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r +
controlModel.getBodyPositionGlobal(indexFootL[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r +
controlModel.getBodyPositionGlobal(indexFootR[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter = (
getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') +
getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4.
#footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(), 0,
viewer.objectInfoWnd.comOffsetZ.value()
])
#footCenter += footCenterOffset
vecRatio = mm.length(curRelCMVec) * 0.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
######################
# optimization terms
######################
# linear momentum
CM_ref_plane = footCenter + footCenterOffset
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] -
CM[1]) - Dl * totalMass * dCM[1]
# angular momentum
CP_ref = footCenter + footCenterOffset
timeStep = 30.
if CP_old[0] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / timeStep)
CP_old[0] = CP
if CP != None and dCP != None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross(
(CP_des - CM_plane),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
flagContact = True
if dH_des == None or np.any(np.isnan(dH_des)) == True:
flagContact = False
#viewer.doc.showRenderer('rd_grf_des', False)
#viewer.motionViewWnd.update(1, viewer.doc)
#else:
#viewer.doc.showRenderer('rd_grf_des', True)
#viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if 0: #stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt * 2
#######################
# optimization
#######################
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
#if flagContact == True:
# mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
# mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
#if Jsup_2 != None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
timeReport[5] += time.time() - curTime
curTime = time.time()
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
vpWorld.step()
if frame % 30 == 0:
print 'elapsed time for 30 frames:', time.time() - pt[0]
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - CM_plane)
rd_CM_des[0] = CM_ref_plane.copy()
rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
if CP != None and dCP != None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
#indexL = motion[0].skeleton.getJointIndex('Hips')
#indexR = motion[0].skeleton.getJointIndex('Spine1')
indexL = indexFootL[0]
indexR = indexFootR[0]
curAng = [controlModel.getBodyOrientationGlobal(indexL)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footL_vec[0] = np.copy(curAngY[0])
rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL)
curAng = [controlModel.getBodyOrientationGlobal(indexR)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footR_vec[0] = np.copy(curAngY[0])
rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR)
if (forceApplyFrame == 0):
applyedExtraForce[0] = [0, 0, 0]
timeReport[6] += time.time() - curTime
# print timeReport
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
