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 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 _calcJointLocalR(joint, extendedPoints, initRot, jointPosture):
if len(joint.children)!=1:
curRot = mmMath.i_so3()
else:
p = extendedPoints[_getValidParentName(joint)] - extendedPoints[joint.name]
c = extendedPoints[joint.children[0].name] - extendedPoints[joint.name]
curRot = mmMath.getSO3FromVectors(p, c)
jointPosture.setLocalR(joint.name, numpy.dot(curRot, initRot[joint.name].transpose()))
for childJoint in joint.children:
_calcJointLocalR(childJoint, extendedPoints, initRot, jointPosture)
def _calcJointLocalR(joint, extendedPoints, initRot, jointPosture):
if len(joint.children)!=1:
curRot = mmMath.I_SO3()
else:
p = extendedPoints[_getValidParentName(joint)] - extendedPoints[joint.name]
c = extendedPoints[joint.children[0].name] - extendedPoints[joint.name]
curRot = mmMath.getSO3FromVectors(p, c)
jointPosture.setLocalR(joint.name, numpy.dot(curRot, initRot[joint.name].transpose()))
for childJoint in joint.children:
_calcJointLocalR(childJoint, extendedPoints, initRot, jointPosture)
def mm2Joint_Posture(pointPosture, pointTPose, jointSkeleton):
jointPosture = ym.JointPosture(jointSkeleton)
jointPosture.rootPos = pointPosture.pointMap[jointSkeleton.root.name]
extendedPoints = {}
for jointName, joint in jointSkeleton.joints.items():
if joint.parent == None:
extendedPoints[
'parent_' +
jointName] = pointTPose.pointMap[jointName] + numpy.array(
[1., 0., 0.])
extendedPoints[jointName] = pointTPose.pointMap[jointName]
elif len(joint.parent.children) > 1:
extendedPoints[jointName] = pointTPose.pointMap[joint.parent.name]
else:
extendedPoints[jointName] = pointTPose.pointMap[jointName]
initRot = {}
for jointName, joint in jointSkeleton.joints.items():
if len(joint.children) != 1:
curRot = mmMath.I_SO3()
else:
p = extendedPoints[_getValidParentName(joint)] - extendedPoints[
joint.name]
c = extendedPoints[joint.children[0].name] - extendedPoints[
joint.name]
curRot = mmMath.getSO3FromVectors(p, c)
initRot[joint.name] = curRot
extendedPoints2 = {}
for jointName, joint in jointSkeleton.joints.items():
if joint.parent == None:
extendedPoints2[
'parent_' +
jointName] = pointPosture.pointMap[jointName] + numpy.array(
[1., 0., 0.])
extendedPoints2[jointName] = pointPosture.pointMap[jointName]
elif len(joint.parent.children) > 1:
extendedPoints2[jointName] = pointPosture.pointMap[
joint.parent.name]
else:
extendedPoints2[jointName] = pointPosture.pointMap[jointName]
_calcJointLocalR(jointSkeleton.root, extendedPoints2, initRot,
jointPosture)
return jointPosture
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 mm2Joint_Posture(pointPosture, pointTPose, jointSkeleton):
jointPosture = ym.JointPosture(jointSkeleton)
jointPosture.rootPos = pointPosture.pointMap[jointSkeleton.root.name]
extendedPoints = {}
for jointName, joint in jointSkeleton.joints.items():
if joint.parent==None:
extendedPoints['parent_'+jointName] = pointTPose.pointMap[jointName] + numpy.array([1.,0.,0.])
extendedPoints[jointName] = pointTPose.pointMap[jointName]
elif len(joint.parent.children)>1:
extendedPoints[jointName] = pointTPose.pointMap[joint.parent.name]
else:
extendedPoints[jointName] = pointTPose.pointMap[jointName]
initRot = {}
for jointName, joint in jointSkeleton.joints.items():
if len(joint.children)!=1:
curRot = mmMath.I_SO3()
else:
p = extendedPoints[_getValidParentName(joint)] - extendedPoints[joint.name]
c = extendedPoints[joint.children[0].name] - extendedPoints[joint.name]
curRot = mmMath.getSO3FromVectors(p, c)
initRot[joint.name] = curRot
extendedPoints2 = {}
for jointName, joint in jointSkeleton.joints.items():
if joint.parent==None:
extendedPoints2['parent_'+jointName] = pointPosture.pointMap[jointName] + numpy.array([1.,0.,0.])
extendedPoints2[jointName] = pointPosture.pointMap[jointName]
elif len(joint.parent.children)>1:
extendedPoints2[jointName] = pointPosture.pointMap[joint.parent.name]
else:
extendedPoints2[jointName] = pointPosture.pointMap[jointName]
_calcJointLocalR(jointSkeleton.root, extendedPoints2, initRot, jointPosture)
return jointPosture
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 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
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]
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)
