def getBlendedNextMotion2(nextMotionA, nextMotionB, prevEndPosture, t=None, attachPosition=True, attachOrientation=True):
dA = prevEndPosture - nextMotionA[0]
dB = prevEndPosture - nextMotionB[0]
newNextMotionA = nextMotionA.copy()
newNextMotionB = nextMotionB.copy()
if attachPosition:
p_offset_A = dA.rootPos
p_offset_B = dB.rootPos
# d.disableTranslation()
newNextMotionA.translateByOffset(p_offset_A)
newNextMotionB.translateByOffset(p_offset_B)
if attachOrientation:
R_offset_A = dA.getJointOrientationLocal(0)
R_offset_A = mm.exp(mm.projectionOnVector(mm.logSO3(R_offset_A), mm.v3(0,1,0))) # # project on y axis
R_offset_B = dA.getJointOrientationLocal(0)
R_offset_B = mm.exp(mm.projectionOnVector(mm.logSO3(R_offset_B), mm.v3(0,1,0))) # # project on y axis
# d.disableRotations([0])
newNextMotionA.rotateTrajectory(R_offset_A)
newNextMotionB.rotateTrajectory(R_offset_B)
if t==None:
blendedNextMotion = blendSegmentSmooth(newNextMotionA, newNextMotionB)
else:
blendedNextMotion = blendSegmentFixed(newNextMotionA, newNextMotionB, t)
# del blendedNextMotion[0]
return blendedNextMotion
def getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None]*len(th_r)
# p_r0 = mm.T2p(th_r[0])
# p0 = mm.T2p(th[0])
# v_r0 = dth_r[0][0:3]
# v0 = dth[0][0:3]
# a_r0 = ddth_r[0][0:3]
# th_r0 = mm.T2R(th_r[0])
# th0 = mm.T2R(th[0])
# dth_r0 = dth_r[0][3:6]
# dth0 = dth[0][3:6]
# ddth_r0 = ddth_r[0][3:6]
p_r0 = th_r[0][0]
p0 = th[0][0]
v_r0 = dth_r[0][0:3]
v0 = dth[0][0:3]
a_r0 = ddth_r[0][0:3]
th_r0 = th_r[0][1]
th0 = th[0][1]
dth_r0 = dth_r[0][3:6]
dth0 = dth[0][3:6]
ddth_r0 = ddth_r[0][3:6]
a_des0 = Kt*(p_r0 - p0) + Dt*(v_r0 - v0) + a_r0
ddth_des0 = Kt*(mm.logSO3(np.dot(th0.transpose(), th_r0))) + Dt*(dth_r0 - dth0) + ddth_r0
ddth_des[0] = np.concatenate((a_des0, ddth_des0))
for i in range(1, len(th_r)):
ddth_des[i] = Kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + Dt*(dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
def blendSegmentSmooth(motionSegment0,
motionSegment1,
attachPosition=True,
attachOrientation=True):
motionSegment1 = motionSegment1.copy()
if attachPosition:
p_offset = motionSegment0[0].rootPos - motionSegment1[0].rootPos
motionSegment1.translateByOffset(p_offset)
if attachOrientation:
R_offset = np.dot(motionSegment0[0].localRs[0],
motionSegment1[0].localRs[0].T)
R_offset = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset),
mm.vec3(0, 1, 0))) # # project on y axis
motionSegment1.rotateTrajectory(R_offset)
newMotion = ym.JointMotion([None] * (int(
(len(motionSegment0) + len(motionSegment1)) / 2.)))
# newMotion = ym.JointMotion( [None]*(int( t*len(motionSegment0) + (1-t)*len(motionSegment1)) ) )
df0 = float(len(newMotion)) / len(motionSegment0)
df1 = float(len(newMotion)) / len(motionSegment1)
for frame in range(len(newMotion)):
normalizedFrame = float(frame) / (len(newMotion) - 1)
normalizedFrame2 = yfg.H1(normalizedFrame)
normalizedFrame2 += df0 * yfg.H2(normalizedFrame)
normalizedFrame2 += df1 * yfg.H3(normalizedFrame)
posture0_at_normalizedFrame = motionSegment0.getPostureAt(
normalizedFrame2 * (len(motionSegment0) - 1))
posture1_at_normalizedFrame = motionSegment1.getPostureAt(
normalizedFrame2 * (len(motionSegment1) - 1))
newMotion[frame] = posture0_at_normalizedFrame.blendPosture(
posture1_at_normalizedFrame, normalizedFrame2)
return newMotion
def getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None] * len(th_r)
for i in range(len(th_r)):
ddth_des[i] = Kt * (mm.logSO3(np.dot(th[i].transpose(), th_r[i]))
) + Dt * (dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
def getDesFootAngularAcc(refModel,
controlModel,
footIndex,
Kk,
Dk,
axis=[0, 1, 0],
desAng=[0, 1, 0]):
desAngularAcc = [0, 0, 0]
curAng = [controlModel.getBodyOrientationGlobal(footIndex)]
refAngVel = refModel.getBodyAngVelocityGlobal(footIndex)
curAngVel = controlModel.getBodyAngVelocityGlobal(footIndex)
refAngAcc = (0, 0, 0)
curAngY = np.dot(curAng, np.array(axis))
refAngY = np.array(desAng)
if stage == MOTION_TRACKING + 10:
refAng = [refModel.getBodyOrientationGlobal(footIndex)]
refAngY2 = np.dot(refAng, np.array([0, 1, 0]))
refAngY = refAngY2[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAcc = Kk * aL + Dk * (refAngVel - curAngVel)
return desAngularAcc
def blendSegmentFixed(motionSegment0, motionSegment1, t, attachPosition=True, attachOrientation=True):
motionSegment1 = motionSegment1.copy()
if attachPosition:
p_offset = motionSegment0[0].rootPos - motionSegment1[0].rootPos
motionSegment1.translateByOffset(p_offset)
if attachOrientation:
R_offset = np.dot(motionSegment0[0].localRs[0], motionSegment1[0].localRs[0].T)
R_offset = mm.exp(mm.projectionOnVector(mm.logSO3(R_offset), mm.v3(0,1,0))) # # project on y axis
motionSegment1.rotateTrajectory(R_offset)
# newMotion = ym.JointMotion( [None]*(int( (len(motionSegment 0)+len(motionSegment1))/2.) ) )
newMotion = ym.JointMotion( [None]*(int( (1-t)*len(motionSegment0) + t*len(motionSegment1)) ) )
# df0 = float(len(newMotion)) / len(motionSegment0)
# df1 = float(len(newMotion)) / len(motionSegment1)
for frame in range(len(newMotion)):
normalizedFrame = float(frame)/(len(newMotion)-1)
# normalizedFrame2 = yfg.H1(normalizedFrame)
# normalizedFrame2 += df0*yfg.H2(normalizedFrame)
# normalizedFrame2 += df1*yfg.H3(normalizedFrame)
# posture0_at_normalizedFrame = motionSegment0.getPostureAt(normalizedFrame2*(len(motionSegment0)-1))
# posture1_at_normalizedFrame = motionSegment1.getPostureAt(normalizedFrame2*(len(motionSegment1)-1))
# newMotion[frame] = posture0_at_normalizedFrame.blendPosture(posture1_at_normalizedFrame, normalizedFrame2)
# print normalizedFrame*(len(motionSegment0)-1)
posture0_at_normalizedFrame = motionSegment0.getPostureAt(normalizedFrame*(len(motionSegment0)-1))
posture1_at_normalizedFrame = motionSegment1.getPostureAt(normalizedFrame*(len(motionSegment1)-1))
# newMotion[frame] = posture0_at_normalizedFrame.blendPosture(posture1_at_normalizedFrame, normalizedFrame)
newMotion[frame] = posture0_at_normalizedFrame.blendPosture(posture1_at_normalizedFrame, t)
return newMotion
def getBlendedNextMotion2(nextMotionA,
nextMotionB,
prevEndPosture,
t=None,
attachPosition=True,
attachOrientation=True):
dA = prevEndPosture - nextMotionA[0]
dB = prevEndPosture - nextMotionB[0]
newNextMotionA = nextMotionA.copy()
newNextMotionB = nextMotionB.copy()
if attachPosition:
p_offset_A = dA.rootPos
p_offset_B = dB.rootPos
# d.disableTranslation()
newNextMotionA.translateByOffset(p_offset_A)
newNextMotionB.translateByOffset(p_offset_B)
if attachOrientation:
R_offset_A = dA.getJointOrientationLocal(0)
R_offset_A = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset_A),
mm.vec3(0, 1, 0))) # # project on y axis
R_offset_B = dA.getJointOrientationLocal(0)
R_offset_B = mm.exp(
mm.projectionOnVector(mm.logSO3(R_offset_B),
mm.vec3(0, 1, 0))) # # project on y axis
# d.disableRotations([0])
newNextMotionA.rotateTrajectory(R_offset_A)
newNextMotionB.rotateTrajectory(R_offset_B)
if t is None:
blendedNextMotion = blendSegmentSmooth(newNextMotionA, newNextMotionB)
else:
blendedNextMotion = blendSegmentFixed(newNextMotionA, newNextMotionB,
t)
# del blendedNextMotion[0]
return blendedNextMotion
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
def getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None] * len(th_r)
# p_r0 = mm.T2p(th_r[0])
# p0 = mm.T2p(th[0])
# v_r0 = dth_r[0][0:3]
# v0 = dth[0][0:3]
# a_r0 = ddth_r[0][0:3]
# th_r0 = mm.T2R(th_r[0])
# th0 = mm.T2R(th[0])
# dth_r0 = dth_r[0][3:6]
# dth0 = dth[0][3:6]
# ddth_r0 = ddth_r[0][3:6]
p_r0 = th_r[0][0]
p0 = th[0][0]
v_r0 = dth_r[0][0:3]
v0 = dth[0][0:3]
a_r0 = ddth_r[0][0:3]
th_r0 = th_r[0][1]
th0 = th[0][1]
dth_r0 = dth_r[0][3:6]
dth0 = dth[0][3:6]
ddth_r0 = ddth_r[0][3:6]
a_des0 = Kt * (p_r0 - p0) + Dt * (v_r0 - v0) + a_r0
ddth_des0 = Kt * (mm.logSO3(np.dot(
th0.transpose(), th_r0))) + Dt * (dth_r0 - dth0) + ddth_r0
ddth_des[0] = np.concatenate((a_des0, ddth_des0))
for i in range(1, len(th_r)):
ddth_des[i] = Kt * (mm.logSO3(np.dot(th[i].transpose(), th_r[i]))
) + Dt * (dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
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 extraDraw():
frame = viewer.getCurrentFrame()
for footName in footNames:
# if footName == RFOOT:
if footName == LFOOT:
# if True:
footPos = motion[frame].getPosition(footName)
Rg = motion[frame].getGlobalRFromParent(footName)
# ygh.drawSO3(Rg, footPos, (0,255,0))
logRg = mm.logSO3(Rg)
logRg_sagittal = mm.projectionOnVector(logRg, sagittalAxis)
logRg_lateral = mm.projectionOnVector(logRg,
lateralAxisMap[footName])
ygh.drawVector(logRg_sagittal, footPos, (255, 255, 0))
ygh.drawVector(logRg_lateral, footPos, (0, 255, 255))
for index, contactStates in list(contactStatesMap.items()):
if contactStates[frame]:
ygh.drawPoint(motion.getPosition(index, frame),
(255, 255, 255), 5.)
def drawSO3(SO3, origin=numpy.array([0,0,0]), color=(0,255,0), lineWidth=1.0, name=''):
glEnable(GL_LINE_STIPPLE)
glLineStipple(2, 0xFAFA)
drawVector(mmMath.logSO3(SO3), origin, color, lineWidth)
glDisable(GL_LINE_STIPPLE)
def getDesiredAngAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt):
ddth_des = [None]*len(th_r)
for i in range(len(th_r)):
ddth_des[i] = Kt*(mm.logSO3(np.dot(th[i].transpose(), th_r[i]))) + Dt*(dth_r[i] - dth[i]) + ddth_r[i]
return ddth_des
sagittalAxis = horizontalRight
lateralAxisMap = {LFOOT: horizontalDirection, RFOOT: -horizontalDirection}
size_sagittals_map = {
LFOOT: [None] * len(motion),
RFOOT: [None] * len(motion)
}
size_laterals_map = {
LFOOT: [None] * len(motion),
RFOOT: [None] * len(motion)
}
for i in range(len(motion)):
for footName in footNames:
Rg = motion[i].getGlobalRFromParent(footName)
logRg = mm.logSO3(Rg)
logRg_sagittal = mm.projectionOnVector(logRg, sagittalAxis)
logRg_lateral = mm.projectionOnVector(logRg,
lateralAxisMap[footName])
size_sagittals_map[footName][i] = mm.componentOnVector(
logRg, sagittalAxis)
size_laterals_map[footName][i] = mm.componentOnVector(
logRg, lateralAxisMap[footName])
lPeakFrames, lPeakTypes = yma.getFootPeakFramesAndTypes(
size_sagittals_map[LFOOT], lFootStates, 5)
rPeakFrames, rPeakTypes = yma.getFootPeakFramesAndTypes(
size_sagittals_map[RFOOT], rFootStates, 5)
showL = True
showR = True
def setupQP(self, frame, motion, mcfg, model, world, config, timestep):
motionModel = cvm.VpMotionModel(world, motion[frame], mcfg)
# constants
invdt = 1./timestep
# dofs and flat data structure
totalDOF = model.getTotalDOF()
DOFs = model.getDOFs()
self.totalDOF = totalDOF
self.ddth_des_flat = ype.makeFlatList(totalDOF)
self.ddth_r_flat = ype.makeFlatList(totalDOF)
self.dth_flat = ype.makeFlatList(totalDOF)
self.dth_r_flat = ype.makeFlatList(totalDOF)
self.ddth_sol = ype.makeNestedList(DOFs)
# momentum matrix
linkMasses = model.getBodyMasses()
totalMass = model.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
self.qp.clear()
Vc_tmp = self.Vc_tmp
# tracking
w = self.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
th_r = motion.getDOFPositionsLocal(frame)
th = model.getDOFPositionsLocal()
dth_r = motion.getDOFVelocitiesLocal(frame)
dth = model.getDOFVelocitiesLocal()
ddth_r = motion.getDOFAccelerationsLocal(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, self.Kt, self.Dt)
linkPositions = model.getBodyPositionsGlobal()
linkVelocities = model.getBodyVelocitiesGlobal()
linkAngVelocities = model.getBodyAngVelocitiesGlobal()
linkInertias = model.getBodyInertiasGlobal()
jointPositions = model.getJointPositionsGlobal()
jointAxeses = model.getDOFAxesesLocal()
#linkPositions_ref = motionModel.getBodyPositionsGlobal()
#linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
#linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
#linkInertias_ref = motionModel.getBodyInertiasGlobal()
#jointPositions_ref = motionModel.getJointPositionsGlobal()
#jointAxeses_ref = motionModel.getDOFAxesesLocal()
ype.flatten(ddth_des, self.ddth_des_flat)
ype.flatten(dth, self.dth_flat)
ype.flatten(dth_r, self.dth_r_flat)
ype.flatten(ddth_r, self.ddth_r_flat)
# get CoM
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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
# jacobian
Jsup = yjc.makeEmptyJacobian(DOFs, 1)
dJsup = Jsup.copy()
Jsys_old = None
if self.Jsys != None:
Jsys_old = self.Jsys.copy()
if self.Jsys == None:
self.Jsys = yjc.makeEmptyJacobian(DOFs, model.getBodyNum())
self.dJsys = self.Jsys.copy()
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
yjc.computeJacobian2(self.Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
if Jsys_old ==None:
self.dJsys = self.Jsys-self.Jsys
else:
self.dJsys = (self.Jsys - Jsys_old)*invdt
#yjc.computeJacobianDerivative2(self.dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
#CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
#dCM_ref = yrp.getCM(linkVelocities_ref, linkMasses, totalMass)
#CM_ref_plane = copy.copy(CM_ref); CM_ref_plane[1]=0.
#dCM_ref_plane = copy.copy(dCM_ref); dCM_ref_plane[1]=0.
#P_ref = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions_ref, CM_ref, linkInertias_ref)
#dP_ref = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities_ref, dCM_ref, linkAngVelocities_ref, linkInertias_ref)
# get EoM
totalActuator = totalDOF
invM = np.zeros((totalActuator,totalDOF))
invMc = np.zeros(totalDOF)
model.getInverseEquationOfMotion(invM, invMc)
#print invMc
# contact detection
Ks = 1
Ds = 1
supsupR = motion[0].skeleton.getJointIndex('RightLeg')
supsupL = motion[0].skeleton.getJointIndex('LeftLeg')
supR = motion[0].skeleton.getJointIndex('RightFoot')
supL = motion[0].skeleton.getJointIndex('LeftFoot')
bodyIDsToCheck = range(world.getBodyNum())
#print bodyIDsToCheck
#bodyIDsToCheck = [supsupR, supsupL]
#bodyIDsToCheck = [supR, supL]
mus = [.5]*len(bodyIDsToCheck)
bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcManyPenaltyForce(self.contactPerSide, bodyIDsToCheck, mus, Ks, Ds)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = world.calcOnePenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
#print bodyIDs, contactPositions
footCenterL = model.getBodyPositionGlobal(supL)
footCenterR = model.getBodyPositionGlobal(supR)
footCenter = footCenterL.copy()
footRefCenterL = motionModel.getBodyPositionGlobal(supL)
footRefCenterR = motionModel.getBodyPositionGlobal(supR)
#if supL in bodyIDs:
#if supR in bodyIDs:
#footCenter = footCenterL + (footCenterR-footCenterL)/2.
#else:
#footCenter = footCenterL.copy()
#else:
#if supR in bodyIDs:
#footCenter = footCenterR.copy()
#else:
#footCenter = np.array((0,0,0))
contactL = 1
contactR = 1
if footRefCenterL[1] < 0.2:
if footRefCenterR[1] < 0.2:
footCenter = footCenterL + (footCenterR-footCenterL)/2.
else:
footCenter = footCenterL.copy()
contactR = 0
else:
contactL = 0
if footRefCenterR[1] < 0.2:
footCenter = footCenterR.copy()
else:
footCenter = np.array((0,0,0))
contactR = 0
#print(contactR, contactL)
footCenter[1] = 0.
# linear momentum
CM_ref = footCenter
#CM_ref =
#dL_des_plane = self.Kl*totalMass*(CM_ref - CM) + self.Dl*totalMass*(dCM_ref - dCM)
dL_des_plane = self.Kl*totalMass*(CM_ref - CM) + self.Dl*totalMass*(-dCM)
dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
#bodyIDs, contactPositions, contactPositionLocals, contactForces = world.calcManyPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
#CP = yrp.getCP(contactPositions, contactForces)
CP = yrp.getSimpleCP(contactPositions)
if self.CP_old==None or CP==None:
dCP = None
else:
dCP = (CP - self.CP_old[0])/(1/30.)
self.CP_old = CP
if CP!=None and dCP!=None:
ddCP_des = self.Kh*(CP_ref - CP) - self.Dh*(dCP)
CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2)
dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*np.array((0,-9.8,0))))
#if contactChangeCount >0: # and contactChangeType == 'DtoS':
##dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
#dH_des *= (self.maxContactChangeCount - self.contactChangeCount)/(self.maxContactChangeCount)
##dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
H = np.dot(P, np.dot(self.Jsys, self.dth_flat))
dH_des = -self.Kh*H[3:]
# equality constraints
JcTVc_append = np.zeros((totalDOF, 0))
VcTJc_list = []
VcTdJc_list = []
dVcTJc_list = []
ac_offset_list = []
totalContact = 4*len(bodyIDs)
totalProblem = totalDOF+totalActuator+totalContact
preSup = -1
for i in range(len(contactPositions)):
sup = bodyIDs[i]
supJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, sup)]
if preSup != sup:
bodyPos = linkPositions[sup]
bodyVel = linkVelocities[sup]
#yjc.computeJacobian2(Jsup, DOFs, jointPositions, jointAxeses, [bodyPos], supJointMasks)
#yjc.computeJacobianDerivative2(dJsup, DOFs, jointPositions, jointAxeses, linkAngVelocities, [bodyPos], supJointMasks)
Jsup = getPartJacobian(self.Jsys, sup)
dJsup = getPartJacobian(self.dJsys, sup)
R_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-bodyPos)) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) )
dR_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-bodyVel)) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) )
#R_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-contactPositions[i])) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) )
#dR_dAd = np.hstack( (np.vstack( (np.eye(3), mm.getCrossMatrixForm(-contactVelocities[i])) ), np.vstack( (np.zeros((3,3)), np.eye(3)) ) ) )
p = contactPositions[i]
dotp = contactVelocities[i]
VcT_tmp = np.zeros((4,6))
dVcT_tmp = VcT_tmp.copy()
for ii in range(4):
n = Vc_tmp[ii]
pcn = np.cross(contactPositions[i], Vc_tmp[ii])
VcT_tmp[ii][:3] =n
VcT_tmp[ii][3:] =pcn
dotpcn = np.cross(contactVelocities[i], Vc_tmp[ii])
dVcT_tmp[ii][3:] = dotpcn
Vc = np.dot(R_dAd, VcT_tmp.T)
dVc = np.dot(R_dAd, dVcT_tmp.T) + np.dot(dR_dAd, VcT_tmp.T)
JcTVc = np.dot( Jsup.T, Vc)
JcTVc_append = np.hstack((JcTVc_append, JcTVc))
VcTJc_list.append( JcTVc.T )
VcTdJc_list.append( np.dot(Vc.T, dJsup) )
dVcTJc_list.append( np.dot(dVc.T, Jsup) )
#TODO:
# when friction cones and velocity cones differ?
#JcTVc = np.dot( Jsup.T, VcT.T)
#JcTVc_append = np.hstack((JcTVc_append, JcTVc))
#VcTJc_list.append( JcTVc.T )
#VcTdJc_list.append( np.dot(VcT, dJsup) )
#dVcTJc_list.append( np.dot(dVcT, Jsup) )
penDepth = -0.05-contactPositions[i][1]
if penDepth < 0.:
penDepth = 0.
#penDepth = 0.
ac_offset = 1000.*penDepth*np.ones(4)
ac_offset_list.append(ac_offset)
preSup = sup
extForce = np.zeros(totalActuator)
if self.extDuration > 0:
Jext = yjc.makeEmptyJacobian(DOFs, 1)
extForcePos = model.getBodyPositionGlobal(self.extForceBody)
extJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, self.extForceBody)]
yjc.computeJacobian2(Jext, DOFs, jointPositions, jointAxeses, [extForcePos], extJointMasks)
extForce = np.dot(Jext.T, self.extForce)
self.extDuration -= timestep
if self.extDuration < 0:
self.extDuration = 0
self.addQPEqualityInverseEomConstraint(totalProblem, totalDOF, totalActuator, totalContact, invM, invMc, JcTVc_append, extForce)
# inequality constraints
if totalContact> 0:
self.addQPInequalityContactForceConstraint(totalProblem, totalDOF, totalActuator, totalContact)
self.addQPInequalityVelocityConstraint(totalProblem, totalDOF, totalActuator, totalContact, VcTJc_list, VcTdJc_list,dVcTJc_list, self.dth_flat, ac_offset_list, invdt)
#self.addQPInequalityVelocityConstraint(totalProblem, totalDOF, totalActuator, totalContact, VcTJc_vel_list, VcTdJc_vel_list,dVcTJc_vel_list, self.dth_flat, ac_offset_list, 30.)
torqueMax = 1000.*np.ones(totalActuator-6)
torqueMin = -torqueMax
self.addQPInequalityTorqueConstraint(totalProblem, totalDOF, totalActuator, totalContact, torqueMax, torqueMin)
# objectives
self.addQPTrackingTerms(totalProblem, 0, totalDOF, self.Bt, w, self.ddth_des_flat)
self.addQPTorqueTerms(totalProblem, totalDOF, totalActuator, self.Btau, w)
if totalContact > 0:
self.addQPContactForceTerms(totalProblem, totalDOF+totalActuator, totalContact, self.Bcon)
#if dH_des !=None:
# allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
# yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
# self.addLinearAndAngularBalancigTerms(totalProblem, 0, totalDOF, self.Bl, self.Bh, P, self.Jsys, self.dth_flat, dP, self.dJsys, dL_des_plane, dH_des)
# end effector
#TODO:
eeList = [supR, supL]
#eeList = []
#if totalContact > 0:
for ee in eeList:
eeCenter = model.getBodyPositionGlobal(ee)
eeJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, ee)]
yjc.computeJacobian2(Jsup, DOFs, jointPositions, jointAxeses, [eeCenter], eeJointMasks)
yjc.computeJacobianDerivative2(dJsup, DOFs, jointPositions, jointAxeses, linkAngVelocities, [eeCenter], eeJointMasks, False)
ee_genvel_ref = np.dot(Jsup, self.dth_r_flat)
ee_genacc_ref = np.dot(Jsup, self.ddth_r_flat) + np.dot(dJsup, self.dth_r_flat)
ee_pos_ref = motionModel.getBodyPositionGlobal(ee)
ee_pos = model.getBodyPositionGlobal(ee)
ee_vel_ref = ee_genvel_ref[:3]
ee_vel = model.getBodyVelocityGlobal(ee)
ee_acc_ref = ee_genacc_ref[:3]
ddp_des_pos = self.Ke*( (ee_pos_ref-th_r[0][0]) - (ee_pos-th[0][0]) )
ddp_des_pos += self.De*(ee_vel_ref - ee_vel)
ddp_des_pos += ee_acc_ref
eeOri = model.getBodyOrientationGlobal(ee)
eeAngVel = model.getBodyAngVelocityGlobal(ee)
ee_angvel_ref = ee_genvel_ref[3:]
ee_angacc_ref = ee_genacc_ref[3:]
a_ori_diff = mm.logSO3(mm.getSO3FromVectors(np.dot(eeOri, np.array([0,1,0])), np.array([0,1,0])))
ddp_des_ang = self.Ke*a_ori_diff + self.De*(-eeAngVel)
#ddp_des_ang = self.Ke*a_ori_diff + self.De*(ee_angvel_ref-eeAngVel)
#ddp_des_ang += ee_angacc_ref
ddp_des = np.hstack( (ddp_des_pos, ddp_des_ang) )
#self.addEndEffectorTerms(totalProblem, 0, totalDOF, Jsup, dJsup, self.dth_flat, ddp_des, self.Be)
self.addEqualityEndEffectorTerms(totalProblem, 0, totalDOF, Jsup, dJsup, self.dth_flat, ddp_des, self.Be)
return contactPositions, CP, CM, footCenter, dL_des_plane, CM_ref
seginfos[i]['max_stf_push_frame'] = None
if len(swingFoots) > 0:
pushes = []
for frame in range(start, int((start + end) // 2) + 1):
dCM_tar = yrp.getCM(
motion_ori.getJointVelocitiesGlobal(frame),
bodyMasses, None, uppers)
direction = mm.normalize2(
mm.projectionOnPlane(dCM_tar, (1, 0, 0),
(0, 0, 1)))
directionAxis = np.cross((0, 1, 0), direction)
pushes.append(
mm.componentOnVector(
mm.logSO3(motion_ori[frame].
getJointOrientationFromParentGlobal(
swingFoots[0])), directionAxis))
seginfos[i]['max_stf_push_frame'] = pushes.index(
max(pushes))
# write .seg
inputName = os.path.basename(path)
root = os.path.splitext(inputName)[0]
outputName = root + '.seg'
outputFile = open(dir + outputName, 'wb')
pickle.dump(seginfos, outputFile)
outputFile.close()
print(outputName, 'done')
pprint.pprint(seginfos)
def simulateCallback(frame):
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
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
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
foreFootCenterL = controlModel.getBodyPositionGlobal(foreSupL)
foreFootCenterR = controlModel.getBodyPositionGlobal(foreSupR)
rearFootCenterL = controlModel.getBodyPositionGlobal(rearSupL)
rearFootCenterR = controlModel.getBodyPositionGlobal(rearSupR)
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)
#dJsys = (Jsys - JsysPre)/(1/30.)
#JsysPre = Jsys
yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
linkPositions, allLinkJointMasks)
if g_initFlag == 0:
softConstPoint = controlModel.getBodyPositionGlobal(constBody)
softConstPoint[1] -= .3
g_initFlag = 1
yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses,
[footCenterL], supLJointMasks)
#dJsupL = (JsupL - JsupPreL)/(1/30.)
#JsupPreL = JsupL
yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[footCenterL], supLJointMasks, False)
yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses,
[footCenterR], supRJointMasks)
#dJsupR = (JsupR - JsupPreR)/(1/30.)
#JsupPreR = JsupR
yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions,
jointAxeses, linkAngVelocities,
[footCenterR], supRJointMasks, 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)
flagForeSupLContact = 0
flagForeSupRContact = 0
flagRearSupLContact = 0
flagRearSupLContact = 0
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105)
index = controlModel.id2index(bodyIDs[i])
if index == foreSupL:
flagForeSupLContact = 1
yjc.computeJacobian2(JforeSupL, DOFs, jointPositions,
jointAxeses, [foreFootCenterL],
foreSupLJointMasks)
yjc.computeJacobianDerivative2(dJforeSupL, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[foreFootCenterL],
foreSupLJointMasks, False)
elif index == foreSupR:
flagForeSupRContact = 1
yjc.computeJacobian2(JforeSupR, DOFs, jointPositions,
jointAxeses, [foreFootCenterR],
foreSupRJointMasks)
yjc.computeJacobianDerivative2(dJforeSupR, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[foreFootCenterR],
foreSupRJointMasks, False)
elif index == rearSupL:
flagRearSupLContact = 1
yjc.computeJacobian2(JrearSupL, DOFs, jointPositions,
jointAxeses, [rearFootCenterL],
rearSupLJointMasks)
yjc.computeJacobianDerivative2(dJrearSupL, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[rearFootCenterL],
rearSupLJointMasks, False)
elif index == rearSupR:
flagRearSupRContact = 1
yjc.computeJacobian2(JrearSupR, DOFs, jointPositions,
jointAxeses, [rearFootCenterR],
rearSupRJointMasks)
yjc.computeJacobianDerivative2(dJrearSupR, DOFs,
jointPositions, jointAxeses,
linkAngVelocities,
[rearFootCenterR],
rearSupRJointMasks, False)
#
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
CMP = yrp.getCMP(contactForces, CM)
r = [0, 0, 0]
if CP != None and np.any(np.isnan(CMP)) != True:
r = CP - CMP
# 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_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,
[softConstPoint], 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,
[softConstPoint], 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]
elif mit.MOTION == mit.TAEKWONDO:
frame_index = [130, 100]
else:
frame_index = [1000000, 1000000]
#MOTION = TAEKWONDO
#frame_index = [135, 100]
if frame > frame_index[0]:
stage = POWERFUL_BALANCING
Kk = Kk * 2
Dk = 2 * (Kk**.5)
elif frame > frame_index[1]:
stage = MOTION_TRACKING
trackingW = w
if stage == MOTION_TRACKING:
trackingW = w2
# 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)
##############################
# Hard constraint
desLinearAccL = getDesFootLinearAcc(motionModel, controlModel, supL,
ModelOffset, CM_ref, CM, Kk, Dk)
desLinearAccR = getDesFootLinearAcc(motionModel, controlModel, supR,
ModelOffset, CM_ref, CM, Kk, Dk)
desAngularAccL = getDesFootAngularAcc(motionModel, controlModel, supL,
Kk, Dk)
desAngularAccR = getDesFootAngularAcc(motionModel, controlModel, supR,
Kk, Dk)
'''
desLinearAccL = [0,0,0]
desAngularAccL = [0,0,0]
desLinearAccR = [0,0,0]
desAngularAccR = [0,0,0]
refPos = motionModel.getBodyPositionGlobal(supL)
curPos = controlModel.getBodyPositionGlobal(supL)
refRootPos = motionModel.getBodyPositionGlobal(rootB)
curRootPos = controlModel.getBodyPositionGlobal(rootB)
refVecL = refPos - CM_ref
if stage == MOTION_TRACKING:
refPos = CM + refVecL
refPos[1] += 0.05
refPos[0] -= 0.05
elif stage == POWERFUL_BALANCING:
refPos = copy.copy(curPos)
refPos[1] = 0
else:
refPos[0] += ModelOffset[0]
rd_root[0] = curRootPos
refVel = motionModel.getBodyVelocityGlobal(supL)
rd_footCenterL[0] = copy.copy(curPos)
#rd_footCenterL[0][2] += 0.2
curVel = controlModel.getBodyVelocityGlobal(supL)
#refAcc = (0,0,0)
refAcc = motionModel.getBodyAccelerationGlobal(supL)
if stage != MOTION_TRACKING:
refPos[1] = 0
if refPos[1] < 0.0 :
refPos[1] = 0.0
rd_DesPosL[0] = refPos
desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
refPos = motionModel.getBodyPositionGlobal(supR)
curPos = controlModel.getBodyPositionGlobal(supR)
refVecR = refPos - CM_ref
if stage == MOTION_TRACKING:
refPos = CM + refVecR
refPos[1] += 0.05
refPos[0] -= 0.05
elif stage == POWERFUL_BALANCING:
refPos = copy.copy(curPos)
refPos[1] = 0
else :
refPos[0] += ModelOffset[0]
refVel = motionModel.getBodyVelocityGlobal(supR)
curVel = controlModel.getBodyVelocityGlobal(supR)
refAcc = motionModel.getBodyAccelerationGlobal(supR)
if stage != MOTION_TRACKING:
refPos[1] = 0
if refPos[1] < 0.0 :
refPos[1] = 0.0
rd_DesPosR[0] = refPos
desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
curAng = [controlModel.getBodyOrientationGlobal(supL)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supL)
curAngVel = controlModel.getBodyAngVelocityGlobal(supL)
refAngAcc = (0,0,0)
curAngY = np.dot(curAng, np.array([0,1,0]))
refAngY = np.array([0,1,0])
if stage == DYNAMIC_BALANCING:
refAng = [motionModel.getBodyOrientationGlobal(supL)]
refAngY2 = np.dot(refAng, np.array([0,1,0]))
refAngY = refAngY2[0]
rd_footL_vec[0] = refAngY
rd_footR_vec[0] = curAngY[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)]
curAng = [controlModel.getBodyOrientationGlobal(supR)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supR)
curAngVel = controlModel.getBodyAngVelocityGlobal(supR)
refAngAcc = (0,0,0)
curAngY = np.dot(curAng, np.array([0,1,0]))
refAngY = np.array([0,1,0])
if stage == DYNAMIC_BALANCING:
refAng = [motionModel.getBodyOrientationGlobal(supR)]
refAngY2 = np.dot(refAng, np.array([0,1,0]))
refAngY = refAngY2[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)]
a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2],
desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]]
'''
a_sup_2 = np.hstack((np.hstack((desLinearAccL, desAngularAccL)),
np.hstack((desLinearAccR, desAngularAccR))))
Jsup_2 = np.vstack((JsupL, JsupR))
dJsup_2 = np.vstack((dJsupL, dJsupR))
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat,
a_sup_2)
##############################
##############################
# Additional constraint
desLinearAccL = [0, 0, 0]
desAngularAccL = [0, 0, 0]
desLinearAccR = [0, 0, 0]
desAngularAccR = [0, 0, 0]
refPos = motionModel.getBodyPositionGlobal(supL)
curPos = controlModel.getBodyPositionGlobal(supL)
refVecL = refPos - CM_ref
if stage == MOTION_TRACKING:
refPos = CM + refVecL
refPos[1] += 0.05
refPos[0] -= 0.05
elif stage == POWERFUL_BALANCING:
refPos = copy.copy(curPos)
refPos[1] = 0
else:
refPos[0] += ModelOffset[0]
refVel = motionModel.getBodyVelocityGlobal(supL)
curVel = controlModel.getBodyVelocityGlobal(supL)
#refAcc = (0,0,0)
refAcc = motionModel.getBodyAccelerationGlobal(supL)
if stage != MOTION_TRACKING:
refPos[1] = 0
if refPos[1] < 0.0:
refPos[1] = 0.0
desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel,
curVel, refAcc, Kk, Dk)
curAng = [controlModel.getBodyOrientationGlobal(supL)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supL)
curAngVel = controlModel.getBodyAngVelocityGlobal(supL)
refAngAcc = (0, 0, 0)
curAngY = np.dot(curAng, np.array([0, 1, 0]))
refAngY = np.array([0, 1, 0])
if stage == DYNAMIC_BALANCING:
refAng = [motionModel.getBodyOrientationGlobal(supL)]
refAngY2 = np.dot(refAng, np.array([0, 1, 0]))
refAngY = refAngY2[0]
rd_footL_vec[0] = refAngY
rd_footR_vec[0] = curAngY[0]
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], refAngY))
desAngularAccL = [Kk * aL + Dk * (refAngVel - curAngVel)]
##############################
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)
controlModel.setDOFAccelerations(ddth_sol)
if flagForeSupLContact == 1:
controlModel.setJointAngAccelerationLocal(
foreSupL, (2.1, 0, 0))
if flagForeSupRContact == 1:
controlModel.setJointAngAccelerationLocal(
foreSupR, (2.1, 0, 0))
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 simulateCallback(frame):
global g_initFlag
global preFootCenterL, preFootCenterR
global preFootOrientationL, preFootOrientationR
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
motionModel.update(motion[frame])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, Bsc = viewer.GetParam()
if stage == 3:
Bsc = 0
#Kl *= 1.5
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
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
footCenter[1] = 0.
footCenter_ref = refFootL + (refFootR - refFootL)/2.0
footCenter_ref[1] = 0.
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_plane_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
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)
dJsys = (Jsys - JsysPre)/(1/30.)
JsysPre = Jsys
#yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if g_initFlag == 0:
preFootCenterL = footCenterL
preFootCenterR = footCenterR
preFootCenterL[1] -= 0.02
preFootCenterR[1] -= 0.02
preFootOrientationL = controlModel.getBodyOrientationGlobal(supL)
preFootOrientationR = controlModel.getBodyOrientationGlobal(supR)
softConstPoint = controlModel.getBodyPositionGlobal(constBody)
#softConstPoint[2] += 0.3
#softConstPoint[1] -= 1.1
#softConstPoint[0] += 0.1
softConstPoint[1] -= .3
#softConstPoint[0] -= .1
#softConstPoint[1] -= 1.
#softConstPoint[0] -= .5
g_initFlag = 1
yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
dJsupL = (JsupL - JsupPreL)/(1/30.)
JsupPreL = JsupL
#yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks, False)
yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
dJsupR = (JsupR - JsupPreR)/(1/30.)
JsupPreR = JsupR
#yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks, False)
preFootCenter = preFootCenterL + (preFootCenterR - preFootCenterL)/2.0
preFootCenter[1] = 0
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
# linear momentum
CM_ref_plane = footCenter
#CM_ref_plane = preFootCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
#print("dL_des_plane ", dL_des_plane )
#dL_des_plane[1] = 0.
# 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)))
#dH_des = [0, 0, 0]
else:
dH_des = None
CMP = yrp.getCMP(contactForces, CM)
r = [0,0,0]
if CP!= None and np.any(np.isnan(CMP))!=True :
r = CP - CMP
#print("r.l", mm.length(r))
#Bba = Bh*(mm.length(r))
Bba = Bh
# 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
'''
cmDiff = footCenter - CM_plane
print("cmDiff", cmDiff)
if stage == 3:
softConstPoint +=
'''
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, [softConstPoint], 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, [softConstPoint], constJointMasks, False)
q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
'''
P_des = preFootCenterR
P_cur = controlModel.getBodyPositionGlobal(supR)
P_cur[1] = 0
dP_des = [0, 0, 0]
dP_cur = controlModel.getBodyVelocityGlobal(supR)
ddP_des2 = Kp*(P_des - P_cur) - Dp*(dP_cur - dP_des)
r = P_des - P_cur
#print("r2", r)
I = np.vstack(([1,0,0],[0,1,0],[0,0,1]))
Z = np.hstack((I, mm.getCrossMatrixForm(-r)))
JL, JA = np.vsplit(JsupR, 2)
Q2 = np.dot(Z, JsupR)
q1 = np.dot(JA, dth_flat)
q2 = np.dot(mm.getCrossMatrixForm(q1), np.dot(mm.getCrossMatrixForm(q1), r))
q_bias2 = np.dot(np.dot(Z, dJsupR), dth_flat) + q2
'''
#print("Q1", Q1)
'''
print("ddP_des1", ddP_des1)
q_ddth1 = np.dot(Q1, ddth_c_flat)
print("q_ddth1", q_ddth1)
print("q_bias1", q_bias1)
ddp1 = q_ddth1+q_bias1
print("ddp1", ddp1)
print("diff1", ddP_des1-ddp1)
'''
'''
print("ddP_des2", ddP_des2)
q_ddth2 = np.dot(Q2, ddth_c_flat)
print("q_ddth2", q_ddth2)
print("q_bias2", q_bias2)
ddp2 = q_ddth2+q_bias2
print("ddp2", ddp2)
print("diff2", ddP_des2-ddp2)
'''
##############################
############################
# IK
'''
P_des = preFootCenterL
P_cur = controlModel.getJointPositionGlobal(supL)
r = P_des - P_cur
Q_des = preFootOrientationL
Q_cur = controlModel.getJointOrientationGlobal(supL)
rv = mm.logSO3(np.dot(Q_cur.transpose(), Q_des))
#print("rv", rv)
des_v_sup = (r[0],r[1],r[2], rv[0], rv[1], rv[2])
A_large = np.dot(JsupL.T, JsupL)
b_large = np.dot(JsupL.T, des_v_sup)
des_d_th = npl.lstsq(A_large, b_large)
ype.nested(des_d_th[0], d_th_IK_L)
P_des2 = preFootCenterR
P_cur2 = controlModel.getJointPositionGlobal(supR)
r2 = P_des2 - P_cur2
Q_des2 = preFootOrientationR
Q_cur2 = controlModel.getJointOrientationGlobal(supR)
rv2 = mm.logSO3(np.dot(Q_cur2.transpose(), Q_des2))
#print("Q_des2", Q_des2)
#print("Q_cur2", Q_cur2)
#print("rv2", rv2)
des_v_sup2 = (r2[0],r2[1],r2[2], rv2[0], rv2[1], rv[2])
A_large = np.dot(JsupR.T, JsupR)
b_large = np.dot(JsupR.T, des_v_sup2)
des_d_th = npl.lstsq(A_large, b_large)
ype.nested(des_d_th[0], d_th_IK_R)
for i in range(len(d_th_IK_L)):
for j in range(len(d_th_IK_L[i])):
d_th_IK[i][j] = d_th_IK_L[i][j] + d_th_IK_R[i][j]
th_IK = yct.getIntegralDOF(th, d_th_IK, 1/timeStep)
dd_th_IK = yct.getDesiredDOFAccelerations(th_IK, th, d_th_IK, dth, ddth_r, Kk, Dk)
ype.flatten(d_th_IK, d_th_IK_flat)
ype.flatten(dd_th_IK, dd_th_IK_flat)
'''
############################
flagContact = True
if dH_des==None or np.any(np.isnan(dH_des)) == True:
flagContact = False
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
if flagContact == False :
if stage == 1:
stage = 2
print("fly")
else:
if stage == 0:
stage = 1
print("contact")
elif stage == 2:
stage = 3
print("landing")
if stage == 3:
Bt = Bt*0.8
Bl = Bl*1
# optimization
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
#mot.addTrackingTerms(problem, totalDOF, Bk, w_IK, dd_th_IK_flat)
mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bp, ddP_des2, Q2, q_bias2)
#mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2)
desLinearAccL = [0,0,0]
desAngularAccL = [0,0,0]
desLinearAccR = [0,0,0]
desAngularAccR = [0,0,0]
refPos = motionModel.getBodyPositionGlobal(supL)
refPos[0] += ModelOffset[0]
refPos[1] = 0
refVel = motionModel.getBodyVelocityGlobal(supL)
curPos = controlModel.getBodyPositionGlobal(supL)
#curPos[1] = 0
curVel = controlModel.getBodyVelocityGlobal(supL)
refAcc = (0,0,0)
if stage == 3:
refPos = curPos
refPos[1] = 0
if curPos[1] < 0.0:
curPos[1] = 0
else :
curPos[1] = 0
rd_DesPosL[0] = refPos
#(p_r, p, v_r, v, a_r, Kt, Dt)
desLinearAccL = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
#desLinearAccL[1] = 0
refPos = motionModel.getBodyPositionGlobal(supR)
refPos[0] += ModelOffset[0]
refPos[1] = 0
refVel = motionModel.getBodyVelocityGlobal(supR)
curPos = controlModel.getBodyPositionGlobal(supR)
#curPos[1] = 0
curVel = controlModel.getBodyVelocityGlobal(supR)
if stage == 3:
refPos = curPos
refPos[1] = 0
if curPos[1] < 0.0:
curPos[1] = 0
else :
curPos[1] = 0
rd_DesPosR[0] = refPos
desLinearAccR = yct.getDesiredAcceleration(refPos, curPos, refVel, curVel, refAcc, Kk, Dk)
#desLinearAccR[1] = 0
#(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
refAng = [preFootOrientationL]
curAng = [controlModel.getBodyOrientationGlobal(supL)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supL)
curAngVel = controlModel.getBodyAngVelocityGlobal(supL)
refAngAcc = (0,0,0)
#desAngularAccL = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk)
curAngY = np.dot(curAng, np.array([0,1,0]))
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0])))
print("curAngYL=",curAngY, "aL=", aL)
desAngularAccL = [Kk*aL + Dk*(refAngVel-curAngVel)]
refAng = [preFootOrientationR]
curAng = [controlModel.getBodyOrientationGlobal(supR)]
refAngVel = motionModel.getBodyAngVelocityGlobal(supR)
curAngVel = controlModel.getBodyAngVelocityGlobal(supR)
refAngAcc = (0,0,0)
#desAngularAccR = yct.getDesiredAngAccelerations(refAng, curAng, refAngVel, curAngVel, refAngAcc, Kk, Dk)
curAngY = np.dot(curAng, np.array([0,1,0]))
aL = mm.logSO3(mm.getSO3FromVectors(curAngY[0], np.array([0,1,0])))
desAngularAccR = [Kk*aL + Dk*(refAngVel-curAngVel)]
print("curAngYR=",curAngY, "aL=", aL)
a_sup_2 = [desLinearAccL[0], desLinearAccL[1], desLinearAccL[2], desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2],
desLinearAccR[0], desLinearAccR[1], desLinearAccR[2], desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]]
if stage == 2 :#or stage == 3:
refAccL = motionModel.getBodyAccelerationGlobal(supL)
refAndAccL = motionModel.getBodyAngAccelerationGlobal(supL)
refAccR = motionModel.getBodyAccelerationGlobal(supR)
refAndAccR = motionModel.getBodyAngAccelerationGlobal(supR)
a_sup_2 = [refAccL[0], refAccL[1], refAccL[2], refAndAccL[0], refAndAccL[1], refAndAccL[2],
refAccR[0], refAccR[1], refAccR[2], refAndAccR[0], refAndAccR[1], refAndAccR[2]]
'''
a_sup_2 = [0,0,0, desAngularAccL[0][0], desAngularAccL[0][1], desAngularAccL[0][2],
0,0,0, desAngularAccR[0][0], desAngularAccR[0][1], desAngularAccR[0][2]]
'''
Jsup_2 = np.vstack((JsupL, JsupR))
dJsup_2 = np.vstack((dJsupL, dJsupR))
if flagContact == True:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.setConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_sup2)
#mot.setConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
#mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, d_th_IK_flat, a_sup_2)
'''
jZ = np.dot(dJsup_2.T, dJsup_2)
lamda = 0.001
for i in range(len(jZ)):
for j in range(len(jZ[0])):
if i == j :
jZ[i][j] += lamda
jZInv = npl.pinv(jZ)
jA = np.dot(Jsup_2, np.dot(jZInv, np.dot(dJsup_2.T, -Jsup_2)))
mot.addConstraint2(problem, totalDOF, jA, 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)
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_footCenterL[0] = preFootCenterL
rd_footCenterR[0] = preFootCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_plane_ref.copy()
#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] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
#print("rd_exf_des", rd_exf_des[0])
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(constBody)-softConstPoint
#if (applyedExtraForce[0][0] != 0 or applyedExtraForce[0][1] != 0 or applyedExtraForce[0][2] != 0) :
if (forceApplyFrame == 0) :
applyedExtraForce[0] = [0, 0, 0]
seginfos[i]['swingKnees'] = swingKnees
if start<end:
# box foot
seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
# segmented foot
# seginfos[i]['ground_height'] = min([posture_seg.getJointPositionGlobal(foot)[1] - 0.05 for foot in [lFoot, rFoot] for posture_seg in motion_ori[start+1:end+1]])
seginfos[i]['max_stf_push_frame'] = None
if len(swingFoots)>0:
pushes = []
for frame in range(start, (start+end)/2 + 1):
dCM_tar = yrp.getCM(motion_ori.getJointVelocitiesGlobal(frame), bodyMasses, None, uppers)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
pushes.append(mm.componentOnVector(mm.logSO3(motion_ori[frame].getJointOrientationFromParentGlobal(swingFoots[0])), directionAxis))
seginfos[i]['max_stf_push_frame'] = pushes.index(max(pushes))
# write .seg
inputName = os.path.basename(path)
root = os.path.splitext(inputName)[0]
outputName = root+'.seg'
outputFile = open(dir+outputName, 'w')
cPickle.dump(seginfos, outputFile)
outputFile.close()
print outputName, 'done'
pprint.pprint(seginfos)
print 'FINISHED'
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 _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)
