def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5,
threshold=np.inf,
suppress=True,
linewidth=3000)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
controlModel.translateByOffset(controlToMotionOffset)
controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
pydart.init()
dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
dartModel.set_q(controlModel.get_q())
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_seg_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_seg_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
supL = motion[0].skeleton.getJointIndex(config['supLink1'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
# jacobian
# JsupL = yjc.makeEmptyJacobian(DOFs, 1)
# dJsupL = JsupL.copy()
# JsupPreL = JsupL.copy()
#
# JsupR = yjc.makeEmptyJacobian(DOFs, 1)
# dJsupR = JsupR.copy()
# JsupPreR = JsupR.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
JconstPre = Jconst.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
# mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'dartModel',
yr.DartModelRenderer(dartModel, (150, 150, 255), yr.POLYGON_LINE))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0)))
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0)))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel)
foot_viewer = None # type: FootWindow
# success!!
# initKt = 50
# initKl = 10.1
# initKh = 3.1
# initBl = .1
# initBh = .1
# initSupKt = 21.6
# initFm = 100.0
# success!! -- 2015.2.12. double stance
# initKt = 50
# initKl = 37.1
# initKh = 41.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 165.0
# single stance
# initKt = 25
# initKl = 80.1
# initKh = 10.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
# single stance -> double stance
# initKt = 25
# initKl = 60.
# initKh = 20.
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
initKt = 25
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0',
'Foot_foot_1_0'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [
motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
]
rIDlist = [
motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex(
'RightFoot_foot_1_0')
def get_jacobianbase_and_masks(skeleton, DOFs, joint_idx):
J = yjc.makeEmptyJacobian(DOFs, 1)
joint_masks = [yjc.getLinkJointMask(skeleton, joint_idx)]
return J, joint_masks
###################################
# simulate
###################################
def simulateCallback(frame):
motionModel.update(motion[frame])
# dartModel.update(motion[frame])
dartModel.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
# doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# tracking
# print(len(motion.get_q(frame)))
# print(motion.get_q(frame))
# print(motion.get_dq(frame))
# print(len(controlModel.get_q()))
# print(controlModel.get_q())
# print(controlModel.get_dq())
# print(np.asarray(motion.get_dq(frame)) - np.asarray(controlModel.get_dq()))
# print(np.asarray(dartModel.get_q())[:6])
# print(controlModel.get_q()[:6])
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)
ype.flatten(ddth_des, ddth_des_flat)
# ddth_des_flat = Kt * (motion.get_q(frame) - np.array(controlModel.get_q())) - Dt * np.array(controlModel.get_dq())
ype.flatten(dth, dth_flat)
# dth_flat = np.array(controlModel.get_dq())
#################################################
# jacobian
#################################################
contact_ids = list()
# contact_ids = [supL, supR]
# contact_ids = footIdlist
if foot_viewer.check_om_l.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
J_contacts = [
yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
]
dJ_contacts = [
yjc.makeEmptyJacobian(DOFs, 1) for i in range(len(contact_ids))
]
joint_masks = [
yjc.getLinkJointMask(motion[0].skeleton, joint_idx)
for joint_idx in contact_ids
]
# caution!! body orientation and joint orientation of foot are totally different!!
footOriL = controlModel.getJointOrientationGlobal(supL)
footOriR = controlModel.getJointOrientationGlobal(supR)
# desire footCenter[1] = 0.041135
# desire footCenter[1] = 0.0197
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
refFootVelL = motionModel.getBodyVelocityGlobal(supL)
refFootVelR = motionModel.getBodyVelocityGlobal(supR)
refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
(refFootR - refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
(refFootL - refFootJointL))
is_contact = [1] * len(contact_ids)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] / 30. + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] / 30. + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] / 30. + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] / 30. + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 1
contMotionOffset = th[0][0] - th_r[0][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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
# calculate contact state
# if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
# contact state
# 0: flying 1: right only 2: left only 3: double
# if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
# elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
# if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
# if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
# initialization
if g_initFlag == 0:
# JsysPre = Jsys.copy()
JconstPre = Jconst.copy()
softConstPoint = footCenterR.copy()
# yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
# footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
# footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
# calculate jacobian
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
dJsys = (Jsys - JsysPre) / (1 / 30.)
JsysPre = Jsys.copy()
# # yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
# print(np.dot(Jsys, dth_flat))
vp_legacy = np.dot(Jsys, dth_flat)
# print(Jsys)
# '''
# calculate jacobian using dart
body_num = dartModel.getBodyNum()
Jsys_dart = np.zeros((6 * body_num, totalDOF))
dJsys_dart = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
# body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
# body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
# Jsys[6*i:6*i+6, :] = body_i_jacobian
# dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv
Jsys_dart[6 * i:6 * i + 6, :] = dartModel.getBody(
i).world_jacobian()[range(-3, 3), :]
dJsys_dart[6 * i:6 * i + 6, :] = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
# print(np.dot(Jsys, controlModel.get_dq()))
dart_result = np.dot(Jsys_dart, controlModel.get_dq())
# print(Jsys)
# '''
Jsys_hp = np.zeros_like(Jsys_dart)
for i in range(len(linkPositions)):
Jsys_hp[6 * i:6 * i + 6, :] = controlModel.computeJacobian(
i, linkPositions[i])
# Jsys = Jsys_hp
# print('vpJ : ', vp_legacy)
# print('hpJ : ', np.dot(Jsys_hp, controlModel.get_dq()))
# print('dart: ', dart_result)
# print('vp : ', np.asarray([[controlModel.getBodyVelocityGlobal(i), controlModel.getBodyAngVelocityGlobal(i)] for i in range(controlModel.getBodyNum())]).flatten())
# print(np.linalg.norm(vp_legacy - dart_result))
for i in range(len(J_contacts)):
J_contacts[i] = Jsys[6 * contact_ids[i]:6 * contact_ids[i] + 6, :]
dJ_contacts[i] = dJsys[6 * contact_ids[i]:6 * contact_ids[i] +
6, :]
# yjc.computeJacobian2(J_contacts[i], DOFs, jointPositions, jointAxeses, [contact_body_pos[i]], [joint_masks[i]])
# yjc.computeJacobianDerivative2(
# dJ_contacts[i], DOFs, jointPositions, jointAxeses, linkAngVelocities, [contact_body_pos[i]], [joint_masks[i]])
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter = footCenterL + (footCenterR - footCenterL)/2.0
# if refFootR[1] >doubleTosingleOffset:
# if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
# if contact == 1 or footCenterR[1] > 0.08:
# if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
# elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# set up equality constraint
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
body_ddqs = list(
map(mm.logSO3, [
np.dot(
contact_body_ori[i].T,
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i], mm.unitY()), mm.unitY())))
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
a_oris = [
np.dot(contact_body_ori[i],
mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i]))
for i in range(len(contact_body_ori))
]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
a_sups = [
np.append(
kt_sup *
(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) +
dt_sup * (ref_body_vel[i] - contact_body_vel[i]),
kt_sup * a_oris[i] + dt_sup *
(ref_body_angvel[i] - contact_body_angvel[i]))
for i in range(len(a_oris))
]
# 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)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
# if contact == 2:
# mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
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, dJsup, dth_flat, a_sup)
# mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
# if contact & 1 and contactChangeCount == 0:
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
ype.nested(ddth_sol_flat, 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)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
# controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
dartModel.set_q(controlModel.get_q())
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
# render contact_ids
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
# viewer.play()
viewer.show()
foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300,
500, 'foot contact modifier', controlModel)
foot_viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
# np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
pydart.init()
dartModel = cdm.DartModel(None, None, None, None, 'cart_pole_blade.skel')
dartMotionModel = cdm.DartModel(None, None, None, None,
'cart_pole_blade.skel')
footIdlist = list(
dartMotionModel.skeleton.body('h_' + name).index_in_skeleton()
for name in ['blade_left', 'blade_right'])
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = dartMotionModel.getBodyOrientationGlobal(foot_id)[1, :]
pelvis_pos = dartMotionModel.skeleton.dof_indices(
(["j_pelvis_pos_x", 'j_pelvis_pos_y', 'j_pelvis_pos_z']))
pelvis_x = dartMotionModel.skeleton.dof_indices((["j_pelvis_rot_x"]))
pelvis = dartMotionModel.skeleton.dof_indices(
(["j_pelvis_rot_y", "j_pelvis_rot_z"]))
upper_body = dartMotionModel.skeleton.dof_indices(
["j_abdomen_1", "j_abdomen_2"])
right_leg = dartMotionModel.skeleton.dof_indices([
"j_thigh_right_x", "j_thigh_right_y", "j_thigh_right_z", "j_shin_right"
])
left_leg = dartMotionModel.skeleton.dof_indices(
["j_thigh_left_x", "j_thigh_left_y", "j_thigh_left_z", "j_shin_left"])
arms = dartMotionModel.skeleton.dof_indices(
["j_bicep_left_x", "j_bicep_right_x"])
foot = dartMotionModel.skeleton.dof_indices(
["j_heel_left_1", "j_heel_left_2", "j_heel_right_1", "j_heel_right_2"])
leg_y = dartMotionModel.skeleton.dof_indices(
["j_thigh_right_y", "j_thigh_left_y"])
INIT_ANGLE = 0.09
s0q = np.zeros(dartMotionModel.skeleton.ndofs)
s0q[pelvis_pos] = 0., .92, 0.
# s0q[pelvis] = 0., -0.
# s0q[upper_body] = 0.3, -0.
s0q[right_leg] = -0., -0., 0.9, -1.5
s0q[left_leg] = -INIT_ANGLE, 0., 0.0, 0.0
s0q[foot] = 0., INIT_ANGLE, 0., 0.
# s0q[right_leg] = -0., -0., 0.2, -.4
# s0q[left_leg] = -0., 0., 0.2, -.4
# s0q[foot] = 0.2, 0., 0.2, 0.
# s0q[leg_y] = -0.785, 0.785
s0q[arms] = 1.5, -1.5
dartModel.set_q(s0q)
dartMotionModel.set_q(s0q)
frame_step_size = 1. / 40.
stepsPerFrame = 25
time_step = dartModel.world.time_step()
# wcfg.lockingVel = 0.01
# dartModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (0, 0, 2.0)
dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# parameter
Kt = 25.
Dt = 2. * (Kt**.5)
Kl = 100.
Dl = 2. * (Kt**.5)
Kh = 100.
Dh = 2. * (Kt**.5)
Ks = 20000.
Ds = 2. * (Kt**.5)
Bt = 1.
Bl = 0.1
Bh = 0.13
supL = dartModel.skeleton.body('h_blade_left').index_in_skeleton()
supR = dartModel.skeleton.body('h_blade_right').index_in_skeleton()
selectedBody = dartModel.skeleton.body('h_head').index_in_skeleton()
# momentum matrix
linkMasses = dartModel.getBodyMasses()
print([body.name for body in dartModel.skeleton.bodynodes])
print(linkMasses)
totalMass = dartModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
#a_sup = (0,0,0, 0,0,0) #ori
#a_sup = (0,0,0, 0,0,0) #L
a_supL = (0, 0, 0, 0, 0, 0)
a_supR = (0, 0, 0, 0, 0, 0)
a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
CP_old = [mm.v3(0., 0., 0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
# ddth_des_flat = ype.makeFlatList(totalDOF)
# dth_flat = ype.makeFlatList(totalDOF)
# ddth_sol = ype.makeNestedList(DOFs)
config = dict()
config['weightMap'] = {
'j_scapula_left': .2,
'j_bicep_left': .2,
'j_forearm_left': .2,
'j_hand_left': .2,
'j_scapula_right': .2,
'j_bicep_right': .2,
'j_forearm_right': .2,
'j_hand_right': .2,
'j_abdomen': .6,
'j_spine': .6,
'j_head': .6,
'j_heel_right': .2,
'j_heel_left': .2,
'j_pelvis': 0.5,
'j_thigh_left': 5.,
'j_shin_left': .5,
'j_thigh_right': 5.,
'j_shin_right': .5
}
# viewer
rd_footCenter = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
viewer.setMaxFrame(1000)
#viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
# viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.DartRenderer(dartMotionModel.world, (255, 240, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dartModel.world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 0, 0)))
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0)))
#viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
#viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
#viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
#viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
#viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
#viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
#success!!
#initKt = 50
#initKl = 10.1
#initKh = 3.1
#initBl = .1
#initBh = .1
#initSupKt = 21.6
#initFm = 100.0
#success!! -- 2015.2.12. double stance
#initKt = 50
#initKl = 37.1
#initKh = 41.8
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 165.0
#single stance
#initKt = 25
#initKl = 80.1
#initKh = 10.8
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 50.0
#single stance -> double stance
#initKt = 25
#initKl = 60.
#initKh = 20.
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 50.0
initKt = 25.
# initKl = 11.
# initKh = 22.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
# initSupKt = 2.5
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
#self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
pdcontroller = PDController(dartModel, dartModel.skeleton,
dartModel.world.time_step(), Kt, Dt)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
ik_solver = hikd.numIkSolver(dartMotionModel)
body_num = dartModel.getBodyNum()
# dJsys = np.zeros((6*body_num, totalDOF))
# dJsupL = np.zeros((6, totalDOF))
# dJsupR = np.zeros((6, totalDOF))
# Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))]
###################################
#simulate
###################################
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
# dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2. * (Kt**.5)
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2. * (kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
com_offset_x, com_offset_y, com_offset_z = getParamVals(
['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
print(dartModel.skeleton.get_spd_tau(th_r_flat, Kt, Dt))
#################################################
# jacobian
#################################################
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootVelR = np.zeros(3)
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
# contMotionOffset = dartModel.getBodyPositionGlobal(0) - dartMotionModel.getBodyPositionGlobal(0)
contMotionOffset = controlToMotionOffset
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
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)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
# if refFootR[1] < doubleTosingleOffset:
# contact +=1
# if refFootL[1] < doubleTosingleOffset:
# contact +=2
if refFootR[1] < footHeight:
contact += 1
if refFootL[1] < footHeight:
contact += 2
g_initFlag = 1
contact = 2
# contact = 1 + 2
# calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6 * body_num, totalDOF))
dJsys = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
Jsys[6 * i:6 * i +
6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6 * i:6 * i + 6, :] = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[
range(-3, 3), :]
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[
range(-3, 3), :]
# calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
if contact == 2:
footCenter = footCenterL.copy() + footOffset
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
footCenter[0] += 0.02
preFootCenter = footCenter.copy()
# linear momentum
# CM_ref_plane = footCenter.copy()
# CM_ref_plane += np.array([0., 0.9, 0.])
# dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
kl = np.diagflat([Kl * 5., Kl, Kl * 5.])
dl = np.diagflat([2.2 * Dl, Dl, 2.2 * Dl])
CM_ref = footCenter.copy()
CM_ref[1] = dartMotionModel.getCOM()[1] - 0.1
# CM_ref += np.array((0., com_offset_y, 0.))
# dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM
dL_des_plane = kl.dot(totalMass *
(CM_ref - CM)) - dl.dot(totalMass * dCM)
# angular momentum
CP_ref = footCenter
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / frame_step_size
CP_old[0] = CP
CP_des[0] = None
# if CP_des[0] is None:
# CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * (
frame_step_size**2)
dH_des = np.cross(
CP_des[0] - CM,
dL_des_plane - totalMass * mm.s2v(dartModel.world.gravity()))
# dH_des = np.cross(footCenter - CM, dL_des_plane - totalMass*mm.s2v(dartModel.world.gravity()))
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh * H[3:]
else:
dH_des = None
# set up equality constraint
a_oriL = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])),
np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])),
np.array([0, 1, 0])))
footErrorL = refFootL.copy()
footErrorL[1] = dartModel.getBody(
supL).shapenodes[0].shape.size()[1] / 2.
footErrorL += -footCenterL + contMotionOffset
footErrorR = refFootR.copy()
footErrorR[1] = dartModel.getBody(
supR).shapenodes[0].shape.size()[1] / 2.
footErrorR += -footCenterR + contMotionOffset
a_supL = np.append(
kt_sup * footErrorL + dt_sup * (refFootVelL - footBodyVelL),
kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * footErrorR + dt_sup * (refFootVelR - footBodyVelR),
kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR))
# 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)
#######################################################
# optimization
#######################################################
if LEG_FLEXIBLE:
if contact == 2:
config['weightMap']['j_thigh_right'] = .8
config['weightMap']['j_shin_right'] = .8
config['weightMap']['j_heel_right'] = .8
else:
config['weightMap']['j_thigh_right'] = .1
config['weightMap']['j_shin_right'] = .25
config['weightMap']['j_heel_right'] = .2
if contact == 1:
config['weightMap']['j_thigh_left'] = .8
config['weightMap']['j_shin_left'] = .8
config['weightMap']['j_heel_left'] = .8
else:
config['weightMap']['j_thigh_left'] = .1
config['weightMap']['j_shin_left'] = .25
config['weightMap']['j_heel_left'] = .2
w = mot.getTrackingWeightDart(DOFs, dartModel.skeleton,
config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if contact & 1:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# ddth_sol[:6] = np.zeros(6)
if dH_des is None:
ddth_sol = ddth_des_flat
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
inv_h = 1. / time_step
_bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = [], [], [], []
for iii in range(stepsPerFrame):
_ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(
dartModel.skeleton, ddth_sol, inv_h)
# _ddq, _tau, _bodyIDs, _contactPositions, _contactPositionLocals, _contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h)
# print(frame, i, tau)
dartModel.applyPenaltyForce(_bodyIDs, _contactPositionLocals,
_contactForces)
dartModel.skeleton.set_forces(_tau)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += time_step
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
del bodyIDs[:]
del contactPositions[:]
del contactPositions[:]
del contactPositionLocals[:]
del contactForces[:]
bodyIDs.extend(_bodyIDs)
contactPositions.extend(_contactPositions)
contactPositionLocals.extend(_contactPositionLocals)
contactForces.extend(_contactForces)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des[0]
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(
dartModel.world.gravity())
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5,
threshold=np.inf,
suppress=True,
linewidth=3000)
motionFile = 'wd2_tiptoe.bvh'
motionFile = 'wd2_tiptoe_zygote.bvh'
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(
motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008)
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
vpWorld.SetGlobalDamping(0.999)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
# controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (0., 0., 0)
controlModel.translateByOffset(controlToMotionOffset)
# controlModel_shadow_for_ik.set_q(controlModel.get_q())
# controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
wcfg_ik = copy.deepcopy(wcfg)
vpWorld_ik = cvw.VpWorld(wcfg_ik)
controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg)
vpWorld_ik.initialize()
controlModel_ik.set_q(np.zeros_like(controlModel.get_q()))
controlModel_q = np.zeros_like(controlModel.get_q())
controlModel_q[4] = controlModel_q[4] + 1.2 - 0.24
controlModel.set_q(controlModel_q)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_seg_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_seg_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
supL = motion[0].skeleton.getJointIndex('LeftFoot')
supR = motion[0].skeleton.getJointIndex('RightFoot')
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
# mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_foot_ori = [None]
rd_foot_pos = [None]
rd_root_ori = [None]
rd_root_pos = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_heel_R'] = init
Ts['foot_R'] = init
Ts['heel_R'] = init
Ts['outside_metatarsal_R'] = init
Ts['outside_phalanges_R'] = init
Ts['inside_metatarsal_R'] = init
Ts['inside_phalanges_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_heel_L'] = init
Ts['foot_L'] = init
Ts['heel_L'] = init
Ts['outside_metatarsal_L'] = init
Ts['outside_phalanges_L'] = init
Ts['inside_metatarsal_L'] = init
Ts['inside_phalanges_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
# viewer = ysv.SimpleViewer()
# viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False)
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1920 + 300, 1 + 1080 + 55],
viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'motionModel',
yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.setRendererVisible('motionModel', False)
viewer.doc.addRenderer(
'ikModel',
yr.VpModelRenderer(controlModel_ik, (50, 50, 50), yr.POLYGON_LINE))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
viewer.doc.setRendererVisible('controlModel', False)
skeleton_renderer = None
if SKELETON_ON:
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.))
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)),
color=(230, 230, 230),
offset_draw=(0., -0.0, 0.))
viewer.doc.addRenderer('skeleton', skeleton_renderer)
viewer.doc.setRendererVisible('skeleton', False)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.setRendererVisible('rd_footCenter', False)
viewer.doc.addRenderer('rd_footCenter_ref',
yr.PointsRenderer(rd_footCenter_ref))
viewer.doc.setRendererVisible('rd_footCenter_ref', False)
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_CM_plane', False)
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.setRendererVisible('rd_CP', False)
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.setRendererVisible('rd_CP_des', False)
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_dL_des_plane', False)
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
viewer.doc.setRendererVisible('rd_dH_des', False)
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_CF', False)
viewer.doc.addRenderer(
'rd_foot_ori',
yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_foot_ori', False)
viewer.doc.addRenderer(
'rd_root_ori',
yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_root_ori', False)
viewer.doc.addRenderer(
'extraForceEnable',
yr.JointArrowRenderer(rd_exf_des,
extraForcePos, (255, 0, 0),
lineWidth=0.004,
polygonStyle=yr.POLYGON_FILL))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel)
foot_viewer = None # type: FootWindow
initKt = 25.
# initKt = 60.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
# initSupKt = 17
initSupKt = 22
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.001, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
def setParamVal(paramname, val):
viewer.objectInfoWnd.setVal(paramname, val)
idDic = dict()
for i in range(motion[0].skeleton.getJointNum()):
idDic[motion[0].skeleton.getJointName(i)] = i
# extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0']
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [
motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
]
rIDlist = [
motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex(
'RightFoot_foot_1_0')
# ik_solver = hik.numIkSolver(dartIkModel)
# ik_solver.clear()
# bodyIDsToCheck = rIDlist.copy()
joint_names = [
motion[0].skeleton.getJointName(i)
for i in range(motion[0].skeleton.getJointNum())
]
def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names):
fixed_nested_dof_values = list()
fixed_nested_dof_values.append(nested_dof_values[0])
for i in range(1, len(_DOFs)):
dof = _DOFs[i]
if dof == 1:
node = _mcfg.getNode(_joint_names[i])
axis = mm.unitZ()
if node.jointAxes[0] == 'X':
axis = mm.unitX()
elif node.jointAxes[0] == 'Y':
axis = mm.unitY()
fixed_nested_dof_values.append(
np.array([np.dot(nested_dof_values[i], axis)]))
else:
fixed_nested_dof_values.append(nested_dof_values[i])
return fixed_nested_dof_values
start_frame = 130
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :]
controlModel_ik.set_q(controlModel.get_q())
controlModel.update(motion[0])
###################################
# simulate
###################################
def preFrameCallback_Always(frame):
if False:
viewer.motionViewWnd.glWindow.camera.rotateX = math.pi / 180. * -25.
if frame > start_frame:
viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(
(frame - start_frame) * 3 + 45.)
else:
viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(45.)
viewer.motionViewWnd.glWindow.camera.distance = .4
viewer.motionViewWnd.glWindow.camera.center = \
.5*(controlModel.getBodyPositionGlobal(idDic['RightFoot']) + controlModel.getBodyPositionGlobal(idDic['LeftFoot'])) + np.array([0., -0.05, 0.])
# viewer.motionViewWnd.glWindow.projectionOrtho = True
# viewer.motionViewWnd.glWindow.projectionChanged = True
# viewer.motionViewWnd.glWindow.camera.rotateY = mm.deg2Rad(182.)
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
if False:
if frame == 200:
if motionFile == 'wd2_tiptoe.bvh':
setParamVal('tiptoe angle', 0.3)
if motionFile == 'wd2_tiptoe_zygote.bvh':
setParamVal('tiptoe angle', 0.3)
# elif 210 < frame < 240:
# if motionFile == 'wd2_tiptoe_zygote.bvh':
# setParamVal('com Y offset', 0.01/30. * (frame-110))
elif frame == 400:
setParamVal('com Y offset', 0.)
setParamVal('tiptoe angle', 0.)
elif frame == 430:
foot_viewer.check_all_seg()
# setParamVal('SupKt', 30.)
# elif frame == 400:
# setParamVal('SupKt', 17.)
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
if abs(getParamVal('tiptoe angle')) > 0.001:
tiptoe_angle = getParamVal('tiptoe angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
if getParamVal('left tilt angle') > 0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle / 5.))
elif getParamVal('left tilt angle') < -0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
if getParamVal('right tilt angle') > 0.001:
right_tilt_angle = getParamVal('right tilt angle')
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
elif getParamVal('right tilt angle') < -0.001:
right_tilt_angle = getParamVal('right tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
controlModel.update(motion[frame])
controlModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# 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)
# ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
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)
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
# rendering
for foot_seg_id in footIdlist:
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
for contact_id in contact_ids:
control_model_renderer.body_colors[contact_id] = (255, 0, 0)
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(
selectedBody) - 0.1 * np.array([
viewer.objectInfoWnd.labelForceX.value(), 0.,
viewer.objectInfoWnd.labelForceZ.value()
])
del rd_exf_des[:]
del extraForcePos[:]
extraForcePos.append(
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot']))
rd_exf_des.append(
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_0_0'])
- controlModel_ik.getJointPositionGlobal(idDic['LeftFoot']))
extraForcePos.append(
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot']))
rd_exf_des.append(
controlModel_ik.getJointPositionGlobal(
idDic['LeftFoot_foot_0_1_0']) -
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot']))
extraForcePos.append(
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot']))
rd_exf_des.append(
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_1_0'])
- controlModel_ik.getJointPositionGlobal(idDic['LeftFoot']))
extraForcePos.append(
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_0_0']))
rd_exf_des.append(
controlModel_ik.getJointPositionGlobal(
idDic['LeftFoot_foot_0_0_0']) -
controlModel_ik.getJointPositionGlobal(idDic['LeftFoot_foot_0_0']))
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips'])
Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg'])
Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg'])
Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot'])
Ts['foot_heel_R'] = controlModel.getJointTransform(
idDic['RightFoot'])
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0'])
Ts['outside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0_0'])
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_1_0'])
Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine'])
Ts['head'] = controlModel.getJointTransform(idDic['Spine1'])
Ts['upper_limb_R'] = controlModel.getJointTransform(
idDic['RightArm'])
Ts['lower_limb_R'] = controlModel.getJointTransform(
idDic['RightForeArm'])
Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg'])
Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg'])
Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot'])
Ts['foot_heel_L'] = controlModel.getJointTransform(
idDic['LeftFoot'])
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0'])
Ts['outside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0_0'])
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_1_0'])
Ts['upper_limb_L'] = controlModel.getJointTransform(
idDic['LeftArm'])
Ts['lower_limb_L'] = controlModel.getJointTransform(
idDic['LeftForeArm'])
skeleton_renderer.appendFrameState(Ts)
viewer.setSimulateCallback(simulateCallback)
viewer.setPreFrameCallback_Always(preFrameCallback_Always)
viewer.startTimer(1 / 30.)
# viewer.play()
viewer.show()
foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300,
500, 'foot contact modifier', controlModel)
foot_viewer.show()
foot_viewer.check_op_l.value(True)
foot_viewer.check_ip_l.value(True)
foot_viewer.check_op_r.value(True)
foot_viewer.check_ip_r.value(True)
foot_viewer.check_not_all_seg()
viewer.motionViewWnd.goToFrame(0)
Fl.run()
def main():
# np.set_printoptions(precision=4, linewidth=200)
np.set_printoptions(precision=5,
threshold=np.inf,
suppress=True,
linewidth=3000)
motionFile = 'wd2_tiptoe.bvh'
motionFile = 'wd2_tiptoe_zygote.bvh'
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(motionFile, SEGMENT_FOOT_RAD=0.008)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(
motionFile, SEGMENT_FOOT_MAG=0.01, SEGMENT_FOOT_RAD=0.008)
# motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
motion.translateByOffset(np.array([-1.5378, 0., +0.29121]))
vpWorld = cvw.VpWorld(wcfg)
vpWorld.SetGlobalDamping(0.999)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
# controlModel_shadow_for_ik = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0., 0)
controlModel.translateByOffset(controlToMotionOffset)
# controlModel_shadow_for_ik.set_q(controlModel.get_q())
# controlModel_shadow_for_ik.computeJacobian(0, np.array([0., 0., 0.]))
wcfg_ik = copy.deepcopy(wcfg)
vpWorld_ik = cvw.VpWorld(wcfg_ik)
controlModel_ik = cvm.VpControlModel(vpWorld_ik, motion[0], mcfg)
vpWorld_ik.initialize()
controlModel_ik.set_q(np.zeros_like(controlModel.get_q()))
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
print(totalDOF)
print(controlModel.getTotalMass())
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_seg_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_seg_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
# selectedBody = motion[0].skeleton.getJointIndex(config['end'])
selectedBody = motion[0].skeleton.getJointIndex('Spine')
constBody = motion[0].skeleton.getJointIndex('RightFoot')
supL = motion[0].skeleton.getJointIndex('LeftFoot')
supR = motion[0].skeleton.getJointIndex('RightFoot')
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
CP_old = [mm.v3(0., 0., 0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
# mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenter_ref = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_foot_ori = [None]
rd_foot_pos = [None]
rd_root_ori = [None]
rd_root_pos = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
def makeEmptyBasicSkeletonTransformDict(init=None):
Ts = dict()
Ts['pelvis'] = init
Ts['spine_ribs'] = init
Ts['head'] = init
Ts['thigh_R'] = init
Ts['shin_R'] = init
Ts['foot_heel_R'] = init
Ts['foot_R'] = init
Ts['heel_R'] = init
Ts['outside_metatarsal_R'] = init
Ts['outside_phalanges_R'] = init
Ts['inside_metatarsal_R'] = init
Ts['inside_phalanges_R'] = init
Ts['upper_limb_R'] = init
Ts['lower_limb_R'] = init
Ts['thigh_L'] = init
Ts['shin_L'] = init
Ts['foot_heel_L'] = init
Ts['foot_L'] = init
Ts['heel_L'] = init
Ts['outside_metatarsal_L'] = init
Ts['outside_phalanges_L'] = init
Ts['inside_metatarsal_L'] = init
Ts['inside_phalanges_L'] = init
Ts['upper_limb_L'] = init
Ts['lower_limb_L'] = init
return Ts
# viewer = ysv.SimpleViewer()
# viewer = hsv.hpSimpleViewer(rect=[0, 0, 1024, 768], viewForceWnd=False)
viewer = hsv.hpSimpleViewer(rect=[0, 0, 1920 + 300, 1 + 1080 + 55],
viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('world', yr.VpWorldRenderer(vpWorld,
(150, 150, 150)))
viewer.doc.addRenderer(
'motionModel',
yr.VpModelRenderer(motionModel, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.setRendererVisible('motionModel', False)
viewer.doc.addRenderer(
'ikModel',
yr.VpModelRenderer(controlModel_ik, (150, 150, 255), yr.POLYGON_LINE))
viewer.doc.setRendererVisible('ikModel', False)
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255, 240, 255),
yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
skeleton_renderer = None
if SKELETON_ON:
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), offset_Y=-0.08)
# skeleton_renderer = yr.BasicSkeletonRenderer(makeEmptyBasicSkeletonTransformDict(np.eye(4)), color=(230, 230, 230), offset_draw=(0.8, -0.02, 0.))
skeleton_renderer = yr.BasicSkeletonRenderer(
makeEmptyBasicSkeletonTransformDict(np.eye(4)),
color=(230, 230, 230),
offset_draw=(0., -0.0, 0.))
viewer.doc.addRenderer('skeleton', skeleton_renderer)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.setRendererVisible('rd_footCenter', False)
viewer.doc.addRenderer('rd_footCenter_ref',
yr.PointsRenderer(rd_footCenter_ref))
viewer.doc.setRendererVisible('rd_footCenter_ref', False)
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_CM_plane', False)
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.setRendererVisible('rd_CP', False)
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.setRendererVisible('rd_CP_des', False)
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_dL_des_plane', False)
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
viewer.doc.setRendererVisible('rd_dH_des', False)
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_CF', False)
viewer.doc.addRenderer(
'rd_foot_ori',
yr.OrientationsRenderer(rd_foot_ori, rd_foot_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_foot_ori', False)
viewer.doc.addRenderer(
'rd_root_ori',
yr.OrientationsRenderer(rd_root_ori, rd_root_pos, (255, 255, 0)))
viewer.doc.setRendererVisible('rd_root_ori', False)
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.setRendererVisible('extraForce', False)
# viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.WideArrowRenderer(rd_exfen_des,
extraForcePos, (255, 0, 0),
lineWidth=.05,
fromPoint=False))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300, 400, 'foot contact modifier', controlModel)
foot_viewer = None # type: FootWindow
initKt = 25.
# initKt = 60.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
# initSupKt = 17
initSupKt = 22
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 300., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 1., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.objectInfoWnd.add1DSlider("tiptoe angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("left tilt angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("right tilt angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("heel angle", -0.5, .5, 0.001, 0.)
viewer.objectInfoWnd.add1DSlider("pha angle", -0.5, .5, 0.001, 0.)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(-1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.4)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
def setParamVal(paramname, val):
viewer.objectInfoWnd.setVal(paramname, val)
idDic = dict()
for i in range(motion[0].skeleton.getJointNum()):
idDic[motion[0].skeleton.getJointName(i)] = i
# extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0']
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [
motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
]
rIDlist = [
motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex(
'RightFoot_foot_1_0')
# ik_solver = hik.numIkSolver(dartIkModel)
# ik_solver.clear()
# bodyIDsToCheck = rIDlist.copy()
joint_names = [
motion[0].skeleton.getJointName(i)
for i in range(motion[0].skeleton.getJointNum())
]
def fix_dofs(_DOFs, nested_dof_values, _mcfg, _joint_names):
fixed_nested_dof_values = list()
fixed_nested_dof_values.append(nested_dof_values[0])
for i in range(1, len(_DOFs)):
dof = _DOFs[i]
if dof == 1:
node = _mcfg.getNode(_joint_names[i])
axis = mm.unitZ()
if node.jointAxes[0] == 'X':
axis = mm.unitX()
elif node.jointAxes[0] == 'Y':
axis = mm.unitY()
fixed_nested_dof_values.append(
np.array([np.dot(nested_dof_values[i], axis)]))
else:
fixed_nested_dof_values.append(nested_dof_values[i])
return fixed_nested_dof_values
start_frame = 200
up_vec_in_each_link = dict()
for foot_id in footIdlist:
up_vec_in_each_link[
foot_id] = controlModel_ik.getBodyOrientationGlobal(foot_id)[1, :]
controlModel_ik.set_q(controlModel.get_q())
###################################
# simulate
###################################
def preFrameCallback_Always(frame):
# print(mm.rad2Deg(math.pi/6.*math.sin((frame-30)*math.pi/180.)))
if frame <= start_frame:
vpWorld.set_plane(0, mm.unitY(), np.zeros(3))
if frame > start_frame:
if math.sin((frame - start_frame) / 360. * math.pi) > 0.:
if frame < start_frame + 50:
setParamVal(
'com Z offset', 0.02 *
math.sin(2. * (frame - start_frame) / 360. * math.pi))
else:
setParamVal('com Z offset', 0.0)
if math.sin((frame - start_frame) / 360. * math.pi) > 0.:
foot_viewer.check_not_all_seg()
foot_viewer.check_tiptoe_all()
setParamVal(
'tiptoe angle',
mm.deg2Rad(10.) * math.sin(
(frame - start_frame) / 360. * math.pi))
# foot_viewer.check_h_l.value(False)
# foot_viewer.check_h_r.value(False)
else:
foot_viewer.check_all_seg()
# foot_viewer.check_tiptoe_all()
# foot_viewer.check_h_l.value(True)
# foot_viewer.check_h_r.value(True)
vpWorld.set_plane(
0,
np.dot(
mm.exp(
-mm.unitX(),
mm.deg2Rad(10.) * math.sin(
(frame - start_frame) / 360. * math.pi)),
mm.unitY()), np.zeros(3))
plane_list = vpWorld.get_plane_list()
plane_normal = plane_list[0][0]
plane_origin = plane_list[0][1]
viewer.motionViewWnd.glWindow.pOnPlaneshadow = plane_origin + plane_normal * 0.001
viewer.motionViewWnd.glWindow.normalshadow = plane_normal
def simulateCallback(frame):
# print(frame)
# print(motion[frame].getJointOrientationLocal(footIdDic['RightFoot_foot_0_1_0']))
# hfi.footAdjust(motion[frame], idDic, SEGMENT_FOOT_MAG=.03, SEGMENT_FOOT_RAD=.015, baseHeight=0.02)
if frame == 0:
setParamVal('com Y offset', 0.02)
if abs(getParamVal('tiptoe angle')) > 0.001:
tiptoe_angle = getParamVal('tiptoe angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), -math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle * 0.95))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot'], mm.exp(mm.unitX(), math.pi * tiptoe_angle))
if getParamVal('left tilt angle') > 0.001:
left_tilt_angle = getParamVal('left tilt angle')
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
elif getParamVal('left tilt angle') < -0.001:
left_tilt_angle = getParamVal('left tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_0'],
mm.exp(mm.unitZ(), -math.pi * left_tilt_angle))
if motion[0].skeleton.getJointIndex(
'LeftFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * left_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot'], mm.exp(mm.unitZ(),
math.pi * left_tilt_angle))
if getParamVal('right tilt angle') > 0.001:
right_tilt_angle = getParamVal('right tilt angle')
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
else:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
elif getParamVal('right tilt angle') < -0.001:
right_tilt_angle = getParamVal('right tilt angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_0'],
mm.exp(mm.unitZ(), math.pi * right_tilt_angle))
if motion[0].skeleton.getJointIndex(
'RightFoot_foot_0_1') is not None:
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
# else:
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
motion[frame].mulJointOrientationLocal(
idDic['RightFoot'],
mm.exp(mm.unitZ(), -math.pi * right_tilt_angle))
if abs(getParamVal('heel angle')) > 0.001:
heel_angle = getParamVal('heel angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_1_0'],
mm.exp(mm.unitX(), math.pi * heel_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_1_0'],
mm.exp(mm.unitX(), math.pi * heel_angle))
if abs(getParamVal('pha angle')) > 0.001:
pha_angle = getParamVal('pha angle')
motion[frame].mulJointOrientationLocal(
idDic['RightFoot_foot_0_1_0'],
mm.exp(mm.unitX(), math.pi * pha_angle))
motion[frame].mulJointOrientationLocal(
idDic['LeftFoot_foot_0_1_0'],
mm.exp(mm.unitX(), math.pi * pha_angle))
# motion[frame].mulJointOrientationLocal(idDic['RightFoot_foot_0_1_0'], mm.exp(mm.unitZ(), -math.pi * pha_angle))
# motion[frame].mulJointOrientationLocal(idDic['LeftFoot_foot_0_1_0'], mm.exp(mm.unitZ(), math.pi * pha_angle))
motionModel.update(motion[frame])
motionModel.translateByOffset(
np.array([
getParamVal('com X offset'),
getParamVal('com Y offset'),
getParamVal('com Z offset')
]))
controlModel_ik.set_q(controlModel.get_q())
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2 * (Kt**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
dt_sup = 2 * (kt_sup**.5)
# 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)
# ype.flatten(fix_dofs(DOFs, ddth_des, mcfg, joint_names), ddth_des_flat)
# ype.flatten(fix_dofs(DOFs, dth, mcfg, joint_names), dth_flat)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
contact_des_ids = list() # desired contact segments
if foot_viewer.check_om_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0'))
if foot_viewer.check_op_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_0_0'))
if foot_viewer.check_im_l is not None and foot_viewer.check_im_l.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1'))
if foot_viewer.check_ip_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_0_1_0'))
if foot_viewer.check_h_l.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0'))
if foot_viewer.check_om_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0'))
if foot_viewer.check_op_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_0_0'))
if foot_viewer.check_im_r is not None and foot_viewer.check_im_r.value(
):
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1'))
if foot_viewer.check_ip_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_0_1_0'))
if foot_viewer.check_h_r.value():
contact_des_ids.append(
motion[0].skeleton.getJointIndex('RightFoot_foot_1_0'))
contact_ids = list() # temp idx for balancing
contact_ids.extend(contact_des_ids)
contact_joint_ori = list(
map(controlModel.getJointOrientationGlobal, contact_ids))
contact_joint_pos = list(
map(controlModel.getJointPositionGlobal, contact_ids))
contact_body_ori = list(
map(controlModel.getBodyOrientationGlobal, contact_ids))
contact_body_pos = list(
map(controlModel.getBodyPositionGlobal, contact_ids))
contact_body_vel = list(
map(controlModel.getBodyVelocityGlobal, contact_ids))
contact_body_angvel = list(
map(controlModel.getBodyAngVelocityGlobal, contact_ids))
ref_joint_ori = list(
map(motion[frame].getJointOrientationGlobal, contact_ids))
ref_joint_pos = list(
map(motion[frame].getJointPositionGlobal, contact_ids))
ref_joint_vel = [
motion.getJointVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_joint_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_ori = list(
map(motionModel.getBodyOrientationGlobal, contact_ids))
ref_body_pos = list(map(motionModel.getBodyPositionGlobal,
contact_ids))
# ref_body_vel = list(map(controlModel.getBodyVelocityGlobal, contact_ids))
ref_body_angvel = [
motion.getJointAngVelocityGlobal(joint_idx, frame)
for joint_idx in contact_ids
]
ref_body_vel = [
ref_joint_vel[i] +
np.cross(ref_joint_angvel[i], ref_body_pos[i] - ref_joint_pos[i])
for i in range(len(ref_joint_vel))
]
is_contact = [1] * len(contact_ids)
contact_right = len(set(contact_des_ids).intersection(rIDlist)) > 0
contact_left = len(set(contact_des_ids).intersection(lIDlist)) > 0
contMotionOffset = th[0][0] - th_r[0][0]
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
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)
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
J_contacts = [] # type: list[np.ndarray]
dJ_contacts = [] # type: list[np.ndarray]
for contact_id in contact_ids:
J_contacts.append(Jsys[6 * contact_id:6 * contact_id + 6, :])
dJ_contacts.append(dJsys[6 * contact_id:6 * contact_id + 6])
# calculate footCenter
footCenter = sum(contact_body_pos) / len(contact_body_pos) if len(contact_body_pos) > 0 \
else .5 * (controlModel.getBodyPositionGlobal(supL) + controlModel.getBodyPositionGlobal(supR))
footCenter[1] = 0.
# if len(contact_body_pos) > 2:
# hull = ConvexHull(contact_body_pos)
footCenter_ref = sum(ref_body_pos) / len(ref_body_pos) if len(ref_body_pos) > 0 \
else .5 * (motionModel.getBodyPositionGlobal(supL) + motionModel.getBodyPositionGlobal(supR))
footCenter_ref = footCenter_ref + contMotionOffset
# if len(ref_body_pos) > 2:
# hull = ConvexHull(ref_body_pos)
footCenter_ref[1] = 0.
# footCenter[0] = footCenter[0] + getParamVal('com X offset')
# footCenter[1] = footCenter[0] + getParamVal('com Y offset')
# footCenter[2] = footCenter[2] + getParamVal('com Z offset')
# initialization
if g_initFlag == 0:
preFootCenter[0] = footCenter.copy()
g_initFlag = 1
# if contactChangeCount == 0 and np.linalg.norm(footCenter - preFootCenter[0]) > 0.01:
# contactChangeCount += 30
if contactChangeCount > 0:
# change footcenter gradually
footCenter = preFootCenter[0] + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter[0]) / maxContactChangeCount
else:
preFootCenter[0] = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane = footCenter_ref
CM_ref = footCenter + np.array([
getParamVal('com X offset'),
motionModel.getCOM()[1] + getParamVal('com Y offset'),
getParamVal('com Z offset')
])
dL_des_plane = Kl * totalMass * (CM_ref - CM) - Dl * totalMass * dCM
# dL_des_plane = Kl * totalMass * (CM_ref_plane - CM_plane) - Dl * totalMass * dCM_plane
# dL_des_plane[1] = 0.
# print('dCM_plane : ', np.linalg.norm(dCM_plane))
# angular momentum
CP_ref = footCenter
# CP_ref = footCenter_ref
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / 30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
dCP_des = dCP + ddCP_des * (1 / 30.)
CP_des = CP + dCP_des * (1 / 30.)
# CP_des = footCenter
CP_des = CP + dCP * (1 / 30.) + .5 * ddCP_des * ((1 / 30.)**2)
dH_des = np.cross(
(CP_des - CM),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
else:
dH_des = None
# convex hull
contact_pos_2d = np.asarray([
np.array([contactPosition[0], contactPosition[2]])
for contactPosition in contactPositions
])
p = np.array([CM_plane[0], CM_plane[2]])
# hull = None # type: Delaunay
# if contact_pos_2d.shape[0] > 0:
# hull = Delaunay(contact_pos_2d)
# print(hull.find_simplex(p) >= 0)
# set up equality constraint
# 1. calculate desired up vector of each contacting body.
plane_list = vpWorld.get_plane_list()
# 1.2. set des_up_vec to normal direction
des_up_vec = [
np.asarray(plane_list[0][0]) for i in range(len(contact_ids))
]
# TODO:
# logSO3 is just q'', not acceleration.
# To make a_oris acceleration, q'' -> a will be needed
# body_ddqs = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], mm.unitY()), mm.unitY()))) for i in range(len(contact_body_ori))]))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
# a_oris = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
# a_oris = list(map(mm.logSO3, [np.dot(np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY())), contact_body_ori[i].T) for i in range(len(contact_body_ori))]))
a_oris = list(
map(mm.logSO3, [
np.dot(
np.dot(
ref_body_ori[i],
mm.getSO3FromVectors(
np.dot(ref_body_ori[i],
up_vec_in_each_link[contact_ids[i]]),
np.asarray(des_up_vec[i]))), contact_body_ori[i].T)
for i in range(len(contact_body_ori))
]))
body_qs = list(map(mm.logSO3, contact_body_ori))
body_angs = [
np.dot(contact_body_ori[i], contact_body_angvel[i])
for i in range(len(contact_body_ori))
]
body_dqs = [
mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))
]
# a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]
# body_ddq = body_ddqs[0]
# body_ori = contact_body_ori[0]
# body_ang = np.dot(body_ori.T, contact_body_angvel[0])
#
# body_q = mm.logSO3(body_ori)
# body_dq = mm.vel2qd(body_ang, body_q)
# a_ori = np.dot(body_ori, mm.qdd2accel(body_ddq, body_dq, body_q))
# KT_SUP = np.diag([kt_sup/10., kt_sup, kt_sup/10.])
KT_SUP = np.diag([kt_sup / 5., kt_sup, kt_sup / 5.])
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(body_ori, mm.unitY()), mm.unitY()) for body_ori in contact_body_ori]))
# a_oris = list(map(mm.logSO3, [mm.getSO3FromVectors(np.dot(contact_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()) for i in range(len(contact_body_ori))]))
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) + dt_sup*(ref_body_vel[i] - contact_body_vel[i]),
# kt_sup*a_oris[i]+dt_sup*(ref_body_angvel[i]-contact_body_angvel[i])) for i in range(len(a_oris))]
# a_sups = [np.append(kt_sup*(ref_body_pos[i] - contact_body_pos[i] + contMotionOffset) - dt_sup * contact_body_vel[i],
# kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
a_sups = [
np.append(
np.dot(KT_SUP,
(ref_body_pos[i] - contact_body_pos[i] +
contMotionOffset)) - dt_sup * contact_body_vel[i],
kt_sup * a_oris[i] - dt_sup * contact_body_angvel[i])
for i in range(len(a_oris))
]
# a_sups = [np.append(np.dot(KT_SUP, (des_pos[i]-contact_body_pos[i])) - dt_sup * contact_body_vel[i],
# 10.*kt_sup*a_oris[i] - dt_sup * contact_body_angvel[i]) for i in range(len(a_oris))]
# for i in range(len(a_sups)):
# a_sups[i][1] = -kt_sup * des_pos[i][1] - dt_sup * contact_body_vel[i][1]
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact_left and not contact_right:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact_right and not contact_left:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if True:
for c_idx in range(len(contact_ids)):
mot.addConstraint2(problem, totalDOF, J_contacts[c_idx],
dJ_contacts[c_idx], dth_flat,
a_sups[c_idx])
if contactChangeCount > 0:
contactChangeCount = contactChangeCount - 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
ype.nested(ddth_sol_flat, 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)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
controlModel.setDOFAccelerations(ddth_sol)
# controlModel.setDOFAccelerations(ddth_des)
# controlModel.set_ddq(ddth_sol_flat)
# controlModel.set_ddq(ddth_des_flat)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
controlModel_ik.set_q(controlModel.get_q())
# rendering
bodyIDs, geomIDs, positionLocalsForGeom = vpWorld.getContactInfoForcePlate(
bodyIDsToCheck)
for foot_seg_id in range(controlModel.getBodyNum()):
control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
control_model_renderer.geom_colors[foot_seg_id] = [
(255, 240, 255)
] * controlModel.getBodyGeomNum(foot_seg_id)
for i in range(len(geomIDs)):
control_model_renderer.geom_colors[bodyIDs[i]][geomIDs[i]] = (255,
0, 0)
# for foot_seg_id in footIdlist:
# control_model_renderer.body_colors[foot_seg_id] = (255, 240, 255)
#
# for contact_id in contact_ids:
# control_model_renderer.body_colors[contact_id] = (255, 0, 0)
rd_footCenter[0] = footCenter
rd_footCenter_ref[0] = footCenter_ref
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
del rd_foot_ori[:]
del rd_foot_pos[:]
# for seg_foot_id in footIdlist:
# rd_foot_ori.append(controlModel.getJointOrientationGlobal(seg_foot_id))
# rd_foot_pos.append(controlModel.getJointPositionGlobal(seg_foot_id))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supL))
rd_foot_ori.append(controlModel.getJointOrientationGlobal(supR))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supL))
rd_foot_pos.append(controlModel.getJointPositionGlobal(supR))
rd_root_des[0] = rootPos[0]
rd_root_ori[0] = controlModel.getBodyOrientationGlobal(0)
rd_root_pos[0] = controlModel.getBodyPositionGlobal(0)
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
# extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
extraForcePos[0] = controlModel.getBodyPositionGlobal(
selectedBody) - 0.1 * np.array([
viewer.objectInfoWnd.labelForceX.value(), 0.,
viewer.objectInfoWnd.labelForceZ.value()
])
# render contact_ids
# render skeleton
if SKELETON_ON:
Ts = dict()
Ts['pelvis'] = controlModel.getJointTransform(idDic['Hips'])
Ts['thigh_R'] = controlModel.getJointTransform(idDic['RightUpLeg'])
Ts['shin_R'] = controlModel.getJointTransform(idDic['RightLeg'])
Ts['foot_R'] = controlModel.getJointTransform(idDic['RightFoot'])
Ts['foot_heel_R'] = controlModel.getJointTransform(
idDic['RightFoot'])
Ts['heel_R'] = np.eye(4)
Ts['outside_metatarsal_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0'])
Ts['outside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_0_0'])
# Ts['inside_metatarsal_R'] = controlModel.getJointTransform(idDic['RightFoot_foot_0_1'])
Ts['inside_metatarsal_R'] = np.eye(4)
Ts['inside_phalanges_R'] = controlModel.getJointTransform(
idDic['RightFoot_foot_0_1_0'])
Ts['spine_ribs'] = controlModel.getJointTransform(idDic['Spine'])
Ts['head'] = controlModel.getJointTransform(idDic['Spine1'])
Ts['upper_limb_R'] = controlModel.getJointTransform(
idDic['RightArm'])
Ts['lower_limb_R'] = controlModel.getJointTransform(
idDic['RightForeArm'])
Ts['thigh_L'] = controlModel.getJointTransform(idDic['LeftUpLeg'])
Ts['shin_L'] = controlModel.getJointTransform(idDic['LeftLeg'])
Ts['foot_L'] = controlModel.getJointTransform(idDic['LeftFoot'])
Ts['foot_heel_L'] = controlModel.getJointTransform(
idDic['LeftFoot'])
Ts['heel_L'] = np.eye(4)
Ts['outside_metatarsal_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0'])
Ts['outside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_0_0'])
# Ts['inside_metatarsal_L'] = controlModel.getJointTransform(idDic['LeftFoot_foot_0_1'])
Ts['inside_metatarsal_L'] = np.eye(4)
Ts['inside_phalanges_L'] = controlModel.getJointTransform(
idDic['LeftFoot_foot_0_1_0'])
Ts['upper_limb_L'] = controlModel.getJointTransform(
idDic['LeftArm'])
Ts['lower_limb_L'] = controlModel.getJointTransform(
idDic['LeftForeArm'])
skeleton_renderer.appendFrameState(Ts)
def postFrameCallback(frame):
if foot_viewer is not None:
foot_viewer.foot_pressure_gl_window.refresh_foot_contact_info(
frame, vpWorld, bodyIDsToCheck, mus, Ks, Ds)
foot_viewer.foot_pressure_gl_window.goToFrame(frame)
viewer.setPreFrameCallback_Always(preFrameCallback_Always)
viewer.setSimulateCallback(simulateCallback)
viewer.setPostFrameCallback_Always(postFrameCallback)
viewer.startTimer(1 / 30.)
# viewer.play()
viewer.show()
foot_viewer = FootWindow(viewer.x() + viewer.w() + 20, viewer.y(), 300,
500, 'foot contact modifier', controlModel)
foot_viewer.show()
foot_viewer.check_all_seg()
viewer.motionViewWnd.goToFrame(0)
Fl.run()
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
#motionName = 'wd2_stand.bvh'
motionName = 'wd2_stand2.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
#addFootSegment
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_3,
(0.045, -0.06, -0.05)) #-0.0037
yme.addJoint(motion, LEFT_TALUS_1, LEFT_METATARSAL_1, (0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_TALUS_3, LEFT_METATARSAL_3,
(0.0, 0.0, 0.1)) #-0.0037
yme.addJoint(motion, LEFT_TALUS_2, LEFT_METATARSAL_2, (0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_METATARSAL_2, LEFT_PHALANGE_2, (0.0, 0.0, 0.07))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1',
(0.0, 0.0, 0.06))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Effector3',
(0.0, 0.0, 0.06))
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2',
(0.0, 0.0, 0.06))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.04))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.04))
#yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.04))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_3, (0.045, -0.06, -0.05))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1',
(0., 0.0, -0.07))
yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2',
(0., 0.0, -0.07))
yme.addJoint(motion, LEFT_CALCANEUS_3, 'LEFT_CALCANEUS_Effector3',
(0., 0.0, -0.07))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, (0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_3, (-0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_METATARSAL_1, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_METATARSAL_2, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_TALUS_3, RIGHT_METATARSAL_3, (0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_METATARSAL_2, RIGHT_PHALANGE_2,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.07))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1',
(0.0, 0.0, 0.06))
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2',
(0.0, 0.0, 0.06))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Effector3',
(0.0, 0.0, 0.06))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.04))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.04))
#yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.04))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, (0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (0.0, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_3, (-0.045, -0.06, -0.05))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1',
(0.0, 0.0, -0.07))
yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2',
(0.0, 0.0, -0.07))
yme.addJoint(motion, RIGHT_CALCANEUS_3, 'RIGHT_CALCANEUS_Effector3',
(0.0, 0.0, -0.07))
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0, 0.0, 0.0), -.5),
False)
yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_3, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0, 0.0, 0.0),
.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .1
node.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .1
node.width = .1
#return mcfg
#
#mass0 = .4
#width0 = 0.028
#length0 = 0.1
#
##Metatarsal1
#length1 = .12
#width1 = 0.03
#mass1 = 0.4
#
#length2 = .1
#width2 = 0.026
#mass2 = 0.4
#
##Metatarsal3
#length3 = .08
#width3 = 0.024
#mass3 = 0.4
#
##Calcaneus1
#length4 = .1
#width4 = 0.032
#mass4 = 0.4
#
##Phalange1
#length5 = .08
#width5 = 0.01
#mass5 = 0.4
##Phalange3
#length7 = length5
#width7 = width5
#mass7 = mass5
#
##Talus
##length8 = .13
##width8 = width0*3
##mass8 = mass0*2.
#
#length8 = .1
#width8 = width0*3
#mass8 = mass0*1.5
width0 = 0.028
length0 = 0.1
mass0 = .4
#Metatarsal1
length1 = .1
width1 = 0.03
mass1 = 0.4
length2 = length1
width2 = width1
mass2 = 0.4
#Metatarsal3
length3 = length1
width3 = width1
mass3 = 0.4
#Calcaneus1
length4 = length1
width4 = width1
mass4 = 0.4
#Phalange1
length5 = length1
width5 = width1
mass5 = 0.4
#Phalange3
length7 = length1
width7 = width1
mass7 = 0.4
#Talus
#length8 = .13
#width8 = width0*3
#mass8 = mass0*2.
length8 = .1
width8 = width0 * 3
mass8 = mass0 * 1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_TALUS_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_3)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_2)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_3)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_TALUS_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_3)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_2)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_METATARSAL_2)
node.length = length2
node.width = width2
node.mass = mass2
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_2)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_3)
node.length = length4
node.width = width4
node.mass = mass4
#node.offset = (0.0, -0.025, 0.0)
node = mcfg.getNode('LeftFoot')
node = mcfg.getNode(RIGHT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_PHALANGE_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_CALCANEUS_3)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_CALCANEUS_3)
node.geom = 'MyFoot3'
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_TALUS_1: .7,
RIGHT_TALUS_1: .7,
LEFT_TALUS_2: .7,
RIGHT_TALUS_2: .7,
LEFT_TALUS_3: .7,
RIGHT_TALUS_3: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_2: .7,
RIGHT_METATARSAL_2: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
RIGHT_CALCANEUS_2: .7,
LEFT_CALCANEUS_2: .7,
RIGHT_CALCANEUS_3: .7,
LEFT_CALCANEUS_3: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_2: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_2: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_TALUS_1: .1,
RIGHT_TALUS_1: .1,
LEFT_TALUS_2: .1,
RIGHT_TALUS_2: .1,
LEFT_TALUS_3: .1,
RIGHT_TALUS_3: .1,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_2: .1,
RIGHT_METATARSAL_2: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
RIGHT_CALCANEUS_2: .2,
LEFT_CALCANEUS_2: .2,
RIGHT_CALCANEUS_3: .2,
LEFT_CALCANEUS_3: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_2: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_2: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
LEFT_TALUS_1:.01, RIGHT_TALUS_1:.01, LEFT_TALUS_2:.01, RIGHT_TALUS_2:.01, LEFT_TALUS_3:.01, RIGHT_TALUS_3:.01,
LEFT_METATARSAL_1:.01, RIGHT_METATARSAL_1:.01, LEFT_METATARSAL_2:.01, RIGHT_METATARSAL_2:.01, LEFT_METATARSAL_3:.01, RIGHT_METATARSAL_3:.01,
RIGHT_CALCANEUS_1:.01, LEFT_CALCANEUS_1:.01, RIGHT_CALCANEUS_2:.01, LEFT_CALCANEUS_2:.01, RIGHT_CALCANEUS_3:.01, LEFT_CALCANEUS_3:.01,
LEFT_PHALANGE_1:.01, LEFT_PHALANGE_2:.01, LEFT_PHALANGE_3:.01, RIGHT_PHALANGE_1:.01, RIGHT_PHALANGE_2:.01, RIGHT_PHALANGE_3:.01}
'''
#config['weightMap']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
# RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
# LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_3:.1, RIGHT_TALUS_3:.1,
# LEFT_METATARSAL_1:.1, RIGHT_METATARSAL_1:.1, LEFT_METATARSAL_3:.1, RIGHT_METATARSAL_3:.1,
# RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_3:.2, LEFT_CALCANEUS_3:.2,
# LEFT_PHALANGE_1:.1, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_3:.1}
#
#config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5,
# RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7,
# LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_3:.7, RIGHT_TALUS_3:.7,
# LEFT_METATARSAL_1:.7, RIGHT_METATARSAL_1:.7, LEFT_METATARSAL_3:.7, RIGHT_METATARSAL_3:.7,
# RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_3:.7, LEFT_CALCANEUS_3:.7,
# LEFT_PHALANGE_1:.4, LEFT_PHALANGE_3:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_3:.4}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [
LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_CALCANEUS_3,
LEFT_TALUS_1, LEFT_TALUS_2, LEFT_TALUS_3, LEFT_METATARSAL_1,
LEFT_METATARSAL_2, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_2,
LEFT_PHALANGE_3
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_CALCANEUS_3,
RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_TALUS_3, RIGHT_METATARSAL_1,
RIGHT_METATARSAL_2, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1,
RIGHT_PHALANGE_2, RIGHT_PHALANGE_3
]
#config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3]
#config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3]
return motion, mcfg, wcfg, stepsPerFrame, config
def main():
np.set_printoptions(precision=4, linewidth=200)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped(
SEGMENT_FOOT, SEGMENT_FOOT_MAG)
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
# motion_offset = np.array((0, 0.15, 0))
# motion.translateByOffset(motion_offset, True)
frame_step_size = 1. / frame_rate
pydart.init()
dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
# wcfg.lockingVel = 0.01
dartModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
# controlToMotionOffset = (1.5, 0.15, 0)
controlToMotionOffset = (1.5, 0.03, 0)
dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(dartModel.getJointNum()):
joint_name = dartModel.getJoint(joint_idx).name
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = dartModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = dartModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
# for dart_foot_joint in (dartModel.getJoint(i) for i in foot_seg_dofs):
# dart_foot_joint.set_actuator_type(pydart.Joint.FORCE)
# parameter
# Kt = config['Kt']; Dt = config['Dt'] # tracking gain
# Kl = config['Kl']; Dl = config['Dl'] # linear balance gain
# Kh = config['Kh']; Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
#
Bt = config['Bt']
# Bl = config['Bl']
# Bh = config['Bh']
# w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink1'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
# momentum matrix
linkMasses = dartModel.getBodyMasses()
print([body.name for body in dartModel.skeleton.bodynodes])
print(linkMasses)
totalMass = dartModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
#a_sup = (0,0,0, 0,0,0) #ori
#a_sup = (0,0,0, 0,0,0) #L
# a_supL = (0,0,0, 0,0,0)
# a_supR = (0,0,0, 0,0,0)
# a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0)
CP_old = [mm.v3(0., 0., 0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
bodyIDsToCheck = range(dartModel.getBodyNum())
#mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
# ddth_des_flat = ype.makeFlatList(totalDOF)
# dth_flat = ype.makeFlatList(totalDOF)
# ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
#viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (0, 255, 255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'motionModel',
yr.DartModelRenderer(dartMotionModel, (150, 150, 255),
yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartModelRenderer(dartModel, (255, 240, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 0, 0)))
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0)))
#viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
#viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
#viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
#viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
#viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
#viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
#success!!
#initKt = 50
#initKl = 10.1
#initKh = 3.1
#initBl = .1
#initBh = .1
#initSupKt = 21.6
#initFm = 100.0
#success!! -- 2015.2.12. double stance
#initKt = 50
#initKl = 37.1
#initKh = 41.8
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 165.0
#single stance
#initKt = 25
#initKl = 80.1
#initKh = 10.8
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 50.0
#single stance -> double stance
#initKt = 25
#initKl = 60.
#initKh = 20.
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 50.0
initKt = 25.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
initFm = 50.0
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
# viewer.objectInfoWnd.labelKt.value(initKt)
# viewer.objectInfoWnd.labelKl.value(initKl)
# viewer.objectInfoWnd.labelKh.value(initKh)
# viewer.objectInfoWnd.labelBl.value(initBl)
# viewer.objectInfoWnd.labelBh.value(initBh)
# viewer.objectInfoWnd.labelSupKt.value(initSupKt)
# viewer.objectInfoWnd.labelFm.value(initFm)
#
# viewer.objectInfoWnd.sliderKt.value(initKt*50)
# viewer.objectInfoWnd.sliderKl.value(initKl*10)
# viewer.objectInfoWnd.sliderKh.value(initKh*10)
# viewer.objectInfoWnd.sliderBl.value(initBl*100)
# viewer.objectInfoWnd.sliderBh.value(initBh*100)
# viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10)
# viewer.objectInfoWnd.sliderFm.value(initFm)
viewer.force_on = False
def viewer_SetForceState(obj):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 7, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 7, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 7, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 7, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
#self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep,
16., 8.)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
extendedFootName = [
'Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0',
'Foot_foot_1_0', 'Foot_foot_1_1', 'Foot_foot_1_2'
]
lIDdic = {
'Left' + name: motion[0].skeleton.getJointIndex('Left' + name)
for name in extendedFootName
}
rIDdic = {
'Right' + name: motion[0].skeleton.getJointIndex('Right' + name)
for name in extendedFootName
}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
###################################
#simulate
###################################
def simulateCallback(frame):
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2. * (Kt**.5)
Dl = 2. * (Kl**.5)
Dh = 2. * (Kh**.5)
dt_sup = 2. * (kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# tracking
# th_r = motion.getDOFPositions(frame)
# th = dartModel.getDOFPositions()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
#caution!! body orientation and joint orientation of foot are totally different!!
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
#desire footCenter[1] = 0.041135
#desire footCenter[1] = 0.0197
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR,
(refFootR - refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL,
(refFootL - refFootJointL))
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(
0) - dartMotionModel.getBodyPositionGlobal(0)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
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)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6 * body_num, totalDOF))
dJsys = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
body_i_jacobian = dartModel.getBody(i).world_jacobian()[
range(-3, 3), :]
body_i_jacobian_deriv = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
Jsys[6 * i:6 * i + 6, :] = body_i_jacobian
dJsys[6 * i:6 * i + 6, :] = body_i_jacobian_deriv
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[
range(-3, 3), :]
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[
range(-3, 3), :]
dartMotionModel.update(motion[frame])
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
ddth_des_flat = pdcontroller.compute(motion.getDOFPositions(frame))
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR)
#if refFootR[1] >doubleTosingleOffset:
#if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
#if contact == 1 or footCenterR[1] > 0.08:
#if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
#change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# foot adjustment
foot_angle_weight = 1.
foot_dCM_weight = 5.
foot_center_diff = CM_plane + dCM_plane * frame_step_size * foot_dCM_weight - footCenter
foot_center_diff_norm = np.linalg.norm(foot_center_diff)
foot_left_height = dartModel.getJointPositionGlobal(foot_left_idx)[1]
foot_right_height = dartModel.getJointPositionGlobal(foot_left_idx)[1]
foot_left_angle = foot_angle_weight * math.atan2(
foot_center_diff_norm, foot_left_height)
foot_right_angle = foot_angle_weight * math.atan2(
foot_center_diff_norm, foot_right_height)
foot_axis = np.cross(np.array((0., 1., 0.)), foot_center_diff)
foot_left_R = mm.exp(foot_axis, foot_left_angle)
foot_right_R = mm.exp(foot_axis, foot_right_angle)
# motion[frame].mulJointOrientationGlobal(foot_left_idx, foot_left_R)
# motion[frame].mulJointOrientationGlobal(foot_right_idx, foot_right_R)
# hfi.footAdjust(motion[frame], footIdDic, SEGMENT_FOOT_MAG, SEGMENT_FOOT_RAD, 0.)
# linear momentum
#TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info(
)
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / frame_step_size
CP_old[0] = CP
# CP_des = None
if CP_des[0] is None:
CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * dCP
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * (
frame_step_size**2)
# dCP_des[0] += ddCP_des * frame_step_size
# CP_des[0] += dCP_des[0] * frame_step_size + .5 * ddCP_des*(frame_step_size ** 2)
dH_des = np.cross(
CP_des[0] - CM,
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
#dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
dH_des *= (maxContactChangeCount -
contactChangeCount) / maxContactChangeCount
#dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh* H[3:]
# soft point constraint
#softConstPoint = refFootR.copy()
##softConstPoint[0] += 0.2
#Ksc = 50
#Dsc = 2*(Ksc**.5)
#Bsc = 1.
#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_des - dP_cur)
#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)
#dJconst = (Jconst - Jconst)/(1/30.)
#JconstPre = Jconst.copy()
##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
#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))
#q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
#set up equality constraint
# a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0])))
# a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0])))
left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(
motion[frame], footIdDic, None)
a_oriL = mm.logSO3(
mm.getSO3FromVectors(left_foot_up_vec, np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(right_foot_up_vec, np.array([0, 1, 0])))
#if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1:
#kt_sup = 30
#viewer.objectInfoWnd.labelSupKt.value(kt_sup)
#viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10)
# a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
# a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup *
(refFootVelL - footBodyVelL),
kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup *
(refFootVelR - footBodyVelR),
kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR))
# a_supL[3:] = 0.
# a_supR[3:] = 0.
if contactChangeCount > 0 and contactChangeType == 'DtoS':
#refFootR += (footCenter-CM_plane)/2.
#refFootR[1] = 0
#pre contact value are needed
#if contact == 2:
##refFootR[0] += 0.2
##refFootR[2] -= 0.05
#offsetDropR = (footCenter-CM_plane)/2.
#refFootR += offsetDropR
#refFootR[1] = 0.
##refFootR[2] = footCenterR[2] - contMotionOffset[2]
##refFootR[0] = footCenterR[0] - contMotionOffset[0]
#refFootL[0] += 0.05
#refFootL[2] -= 0.05
#elif contact == 1:
#offsetDropL = (footCenter-CM_plane)/2.
#refFootL += offsetDropL
#refFootL[1] = 0.
#a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
kt_sup * (refFootL - footCenterL + contMotionOffset) + dt_sup *
(refFootVelL - footBodyVelL), 4 * kt_sup * a_oriL +
2 * dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * (refFootR - footCenterR + contMotionOffset) + dt_sup *
(refFootVelR - footBodyVelR), 4 * kt_sup * a_oriR +
2 * dt_sup * (refFootAngVelR - footBodyAngVelR))
elif contactChangeCount > 0 and contactChangeType == 'StoD':
#refFootR[0] +=0.05
#refFootR[2] +=0.05
linkt = (13. * contactChangeCount) / maxContactChangeCount + 1.
lindt = 2 * (linkt**.5)
angkt = (13. * contactChangeCount) / maxContactChangeCount + 1.
angdt = 2 * (angkt**.5)
#a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(
linkt * kt_sup * (refFootL - footCenterL + contMotionOffset) +
lindt * dt_sup * (refFootVelL - footBodyVelL),
angkt * kt_sup * a_oriL + angdt * dt_sup *
(refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
linkt * kt_sup * (refFootR - footCenterR + contMotionOffset) +
lindt * dt_sup * (refFootVelR - footBodyVelR),
angkt * kt_sup * a_oriR + angdt * dt_sup *
(refFootAngVelR - footBodyAngVelR))
#a_supL = np.append(16*kt_sup*(refFootL - footCenterL + contMotionOffset) + 4*dt_sup*(refFootVelL - footBodyVelL), 16*kt_sup*a_oriL+4*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(16*kt_sup*(refFootR - footCenterR + contMotionOffset) + 4*dt_sup*(refFootVelR - footBodyVelR), 16*kt_sup*a_oriR+4*dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL = np.append(4*kt_sup*(refFootL - footCenterL + contMotionOffset) + 2*dt_sup*(refFootVelL - footBodyVelL), 32*kt_sup*a_oriL+5.6*dt_sup*(refFootAngVelL-footBodyAngVelL))
#a_supR = np.append(4*kt_sup*(refFootR - footCenterR + contMotionOffset) + 2*dt_sup*(refFootVelR - footBodyVelR), 32*kt_sup*a_oriR+5.6*dt_sup*(refFootAngVelR-footBodyAngVelR))
#a_supL[1] = kt_sup*(refFootL[1] - footCenterL[1] + contMotionOffset[1]) + dt_sup*(refFootVelL[1] - footBodyVelL[1])
#a_supR[1] = kt_sup*(refFootR[1] - footCenterR[1] + contMotionOffset[1]) + dt_sup*(refFootVelR[1] - footBodyVelR[1])
##if contact == 2:
#if refFootR[1] <doubleTosingleOffset :
#Jsup = np.vstack((JsupL, JsupR))
#dJsup = np.vstack((dJsupL, dJsupR))
#a_sup = np.append(a_supL, a_supR)
#else:
#Jsup = JsupL.copy()
#dJsup = dJsupL.copy()
#a_sup = a_supL.copy()
# 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)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
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, dJsup, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# remove foot seg effect
ddth_sol[foot_dofs] = ddth_des_flat[foot_dofs]
# ddth_sol[:] = ddth_des_flat[:]
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
for i in range(stepsPerFrame):
# apply penalty force
if not DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
dartModel.skeleton.set_accelerations(ddth_sol)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info(
)
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des[0]
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def create_biped():
# motion
# motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
# motionName = 'wd2_stand.bvh'
motionName = 'wd2_stand2.bvh'
# motionName = 'woddy2_jump0_short.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO :
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2 :
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK :
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK :
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
# motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK :
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0.), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0.), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1., 0., 0.), .01), False)
# yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0,0,1), -.01), False)
# addFootSegment
# Talus
length1 = .15
width1 = .03
mass1 = .4
# Phalange
length3 = length1*0.75
width3 = width1
mass3 = 0.4
length3_2 = length1*0.6
width3_2 = width1
mass3_2 = 0.4
#Calcaneus
length4 = 0.05
width4 = width1
mass4 = 0.4
width1 = 0.03
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_1, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_TALUS_2, ( 0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_1, ( 0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_TALUS_2, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_TALUS_1, LEFT_PHALANGE_1, (0.0, 0.0, length1-width1-width3 ))
yme.addJoint(motion, LEFT_TALUS_2, LEFT_PHALANGE_2, (0.0, 0.0, length1-width1-width3_2))
yme.addJoint(motion, RIGHT_TALUS_1, RIGHT_PHALANGE_1, (0.0, 0.0, length1-width1-width3 ))
yme.addJoint(motion, RIGHT_TALUS_2, RIGHT_PHALANGE_2, (0.0, 0.0, length1-width1-width3_2))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Effector1', (0.0, 0.0, length3 -width3 ))
yme.addJoint(motion, LEFT_PHALANGE_2, 'LEFT_PHALANGE_Effector2', (0.0, 0.0, length3_2-width3_2))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Effector1', (0.0, 0.0, length3 -width3 ))
yme.addJoint(motion, RIGHT_PHALANGE_2, 'RIGHT_PHALANGE_Effector2', (0.0, 0.0, length3_2-width3_2))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_2, ( 0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1, ( 0.025, -0.06, -0.05))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_2, (-0.025, -0.06, -0.05))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Effector1', (0.0, 0.0, -length4))
yme.addJoint(motion, LEFT_CALCANEUS_2, 'LEFT_CALCANEUS_Effector2', (0.0, 0.0, -length4))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Effector1', (0.0, 0.0, -length4))
yme.addJoint(motion, RIGHT_CALCANEUS_2, 'RIGHT_CALCANEUS_Effector2', (0.0, 0.0, -length4))
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_1, mm.exp(mm.v3(1.0,0.0,0.0), .5), False)
yme.rotateJointLocal(motion, RIGHT_TALUS_2, mm.exp(mm.v3(1.0,0.0,0.0), .5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_2, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(0., 1., 0.), 1.57), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_1, mm.exp(mm.v3(1.0,0.0,0.0), .5), False)
yme.rotateJointLocal(motion, LEFT_TALUS_2, mm.exp(mm.v3(1.0,0.0,0.0), .5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(0.0,0.0,1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_2, mm.exp(mm.v3(1.0,0.0,0.0), -.5), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(0.0,-1.0,0.0), 1.57), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]]*40
motion.extend([motion[-1]]*2000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = 1.
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0,0,0.1)
node = mcfg.getNode(SPINE)
node.width = .22
# node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .1
node.width = .1
node = mcfg.getNode('LeftFoot')
node.length = .1
node.width = .1
#Talus
#length1 = .12
#width1 = 0.03
#mass1 = 0.4
#Phalange
#length3 = length1
#width3 = width1
#mass3 = 0.4
#length3_2 = length1*0.8
#width3_2 = width1
#mass3_2 = 0.4
#Calcaneus1
#length4 = 0.1
#width4 = width1
#mass4 = 0.4
#Foot
length8 = .1
width8 = 0.028*3
mass8 = .4*1.5
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_TALUS_1)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_TALUS_2)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.length = length3_2
node.width = width3_2
node.mass = mass3_2
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4 - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.length = length4 - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_TALUS_1)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_TALUS_2)
node.length = length1 - width1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_2)
node.length = length3_2
node.width = width3_2
node.mass = mass3_2
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4# - width4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.length = length4# - width4
node.width = width4
node.mass = mass4
#node.offset = (0.0, -0.025, 0.0)
node = mcfg.getNode('LeftFoot')
node = mcfg.getNode(RIGHT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_TALUS_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_1)
node.geom = 'MyFoot3'
node = mcfg.getNode(LEFT_METATARSAL_2)
node.geom = 'MyFoot3'
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_PHALANGE_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_PHALANGE_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_PHALANGE_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(RIGHT_CALCANEUS_2)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.geom = 'MyFoot4'
node = mcfg.getNode(LEFT_CALCANEUS_2)
node.geom = 'MyFoot4'
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 120
wcfg.timeStep = (1/30.)/(stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.#0.5
config['Bh'] = 1.
config['weightMap'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.2, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.3,
RIGHT_UP_LEG:.1, RIGHT_LEG:.2, LEFT_UP_LEG:.1, LEFT_LEG:.2,
LEFT_TALUS_1:.1, RIGHT_TALUS_1:.1, LEFT_TALUS_2:.1, RIGHT_TALUS_2:.1,
RIGHT_CALCANEUS_1:.2, LEFT_CALCANEUS_1:.2, RIGHT_CALCANEUS_2:.2, LEFT_CALCANEUS_2:.2,
LEFT_PHALANGE_1:.1, LEFT_PHALANGE_2:.1, RIGHT_PHALANGE_1:.1, RIGHT_PHALANGE_2:.1}
config['weightMap2'] = {RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.5, SPINE1:.3, RIGHT_FOOT:.7, LEFT_FOOT:.7, HIP:.5,
RIGHT_UP_LEG:.7, RIGHT_LEG:.7, LEFT_UP_LEG:.7, LEFT_LEG:.7,
LEFT_TALUS_1:.7, RIGHT_TALUS_1:.7, LEFT_TALUS_2:.7, RIGHT_TALUS_2:.7,
RIGHT_CALCANEUS_1:.7, LEFT_CALCANEUS_1:.7, RIGHT_CALCANEUS_2:.7, LEFT_CALCANEUS_2:.7,
LEFT_PHALANGE_1:.4, LEFT_PHALANGE_2:.4, RIGHT_PHALANGE_1:.4, RIGHT_PHALANGE_2:.4}
'''
(1, 'RightUpLeg')
(2, 'RightLeg')
(3, 'RightFoot')
(4, 'Spine')
(5, 'Spine1')
(6, 'LeftArm')
(7, 'LeftForeArm')
(8, 'RightArm')
(9, 'RightForeArm')
(10, 'LeftUpLeg')
(11, 'LeftLeg')
(12, 'LeftFoot')
(13, 'LeftTalus_1')
(14, 'LeftTalus_2')
(15, 'RightTalus_1')
(16, 'RightTalus_2')
(17, 'LeftPhalange_1')
(18, 'LeftPhalange_2')
(19, 'RightPhalange_1')
(20, 'RightPhalange_2')
(21, 'LeftCalcaneus_1')
(22, 'LeftCalcaneus_2')
(23, 'RightCalcaneus_1')
(24, 'RightCalcaneus_2')
'''
config['trackWMap']={10, 5, .1, 20, 10, 5, 2, 10, 5, .1,
0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.10, 0.01}
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
# config['end'] = HIP
config['end'] = SPINE1
config['trunk'] = SPINE
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_CALCANEUS_2, LEFT_TALUS_1, LEFT_TALUS_2, LEFT_PHALANGE_1, LEFT_PHALANGE_2]
config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_CALCANEUS_2, RIGHT_TALUS_1, RIGHT_TALUS_2, RIGHT_PHALANGE_1, RIGHT_PHALANGE_2]
return motion, mcfg, wcfg, stepsPerFrame, config
RIGHT_PHALANGE_2, RIGHT_PHALANGE_3
]
#config['FootLPart'] = [LEFT_FOOT, LEFT_CALCANEUS_1, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1, LEFT_PHALANGE_3]
#config['FootRPart'] = [RIGHT_FOOT, RIGHT_CALCANEUS_1, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3, RIGHT_PHALANGE_1, RIGHT_PHALANGE_3]
return motion, mcfg, wcfg, stepsPerFrame, config
#===============================================================================
# biped config
#===============================================================================
# motion, mesh config
g_motionDirConfigMap = {}
g_motionDirConfigMap['../Data/woody2/Motion/Physics2/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), .05), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.I_SO3()}
g_motionDirConfigMap['../Data/woody2/Motion/Balancing/'] = \
{'footRot': mm.exp(mm.v3(1,-.5,0), -.6), 'yOffset': .0, 'scale':1.,\
'rootRot': mm.exp(mm.v3(1,0,0), .01)}
g_motionDirConfigMap['../Data/woody2/Motion/VideoMotion/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.05), 'yOffset': .01, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .0)}
g_motionDirConfigMap['../Data/woody2/Motion/Samsung/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.03), 'yOffset': .0, 'scale':2.53999905501,\
'rootRot': mm.exp(mm.v3(1,0,0), .03)}
#===============================================================================
# # reloadable config
#===============================================================================
def main():
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped_zygote()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
sphere_radius = 0.5
vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.3209))
vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.3301))
# vpWorld.add_sphere_bump(sphere_radius, (1.6361, -sphere_radius + 0.08, -0.2909))
# vpWorld.add_sphere_bump(sphere_radius, (1.4543, -sphere_radius + 0.08, -0.2901))
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
# controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
controlModel.translateByOffset(controlToMotionOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
# parameter
Kt = config['Kt']; Dt = config['Dt'] # tracking gain
Kl = config['Kl']; Dl = config['Dl'] # linear balance gain
Kh = config['Kh']; Dh = config['Dh'] # angular balance gain
Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink1'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
# selectedBody = motion[0].skeleton.getJointIndex(config['end'])
selectedBody = motion[0].skeleton.getJointIndex('Spine')
constBody = motion[0].skeleton.getJointIndex('RightFoot')
# jacobian
JsupL = yjc.makeEmptyJacobian(DOFs, 1)
dJsupL = JsupL.copy()
JsupPreL = JsupL.copy()
JsupR = yjc.makeEmptyJacobian(DOFs, 1)
dJsupR = JsupR.copy()
JsupPreR = JsupR.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
JconstPre = Jconst.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)]
supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)]
constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
a_supL = (0,0,0, 0,0,0)
a_supR = (0,0,0, 0,0,0)
a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0)
CP_old = [mm.v3(0.,0.,0.)]
# penalty method
bodyIDsToCheck = list(range(vpWorld.getBodyNum()))
mus = [1.]*len(bodyIDsToCheck)
# mus = [.5]*len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(rect=(0, 0, 960+300, 1080+56), viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('world', yr.VpWorldRenderer(vpWorld, (150, 150, 150)))
viewer.doc.addRenderer('motionModel', yr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
viewer.doc.setRendererVisible('motionModel', False)
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('controlModel', yr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.setRendererVisible('rd_footCenter', False)
viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
viewer.doc.setRendererVisible('rd_CM_plane', False)
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0,255,0)))
viewer.doc.setRendererVisible('rd_CP', False)
viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
viewer.doc.setRendererVisible('rd_CP_des', False)
viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
viewer.doc.setRendererVisible('rd_dL_des_plane', False)
viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
viewer.doc.setRendererVisible('rd_dH_des', False)
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255,255,0)))
# viewer.doc.setRendererVisible('rd_CF', False)
viewer.doc.addRenderer('extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0,255,0)))
viewer.doc.setRendererVisible('extraForce', False)
# viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0)))
viewer.doc.addRenderer('extraForceEnable', yr.WideArrowRenderer(rd_exfen_des, extraForcePos, (255,0,0), lineWidth=.05, fromPoint=False))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# success!!
# initKt = 50
# initKl = 10.1
# initKh = 3.1
# initBl = .1
# initBh = .1
# initSupKt = 21.6
# initFm = 100.0
# success!! -- 2015.2.12. double stance
# initKt = 50
# initKl = 37.1
# initKh = 41.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 165.0
# single stance
# initKt = 25
# initKl = 80.1
# initKh = 10.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
# single stance -> double stance
# initKt = 25
# initKl = 60.
# initKh = 20.
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
initKt = 25
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 1., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30+offset*9, 40, 20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30+offset*9, 40, 20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30+offset*9, 40, 20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(-1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(220, 30+offset*9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.4)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
###################################
# simulate
###################################
def simulateCallback(frame):
motionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2*(Kt**.5)
Dl = 2*(Kl**.5)
Dh = 2*(Kh**.5)
dt_sup = 2*(kt_sup**.5)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
# doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# 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)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
# caution!! body orientation and joint orientation of foot are totally different!!
footOriL = controlModel.getJointOrientationGlobal(supL)
footOriR = controlModel.getJointOrientationGlobal(supR)
# desire footCenter[1] = 0.041135
# desire footCenter[1] = 0.0197
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
refFootVelL = motionModel.getBodyVelocityGlobal(supL)
refFootVelR = motionModel.getBodyVelocityGlobal(supR)
refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)
refFootOriL = motionModel.getBodyOrientationGlobal(supL)
refFootOriR = motionModel.getBodyOrientationGlobal(supR)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1]/30. + refFootR[1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1]/30. + refFootL[1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1]/30. + refFootR[1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1]/30. + refFootL[1] > doubleTosingleOffset:
contactL = 0
# if 32 < frame < 147:
# contactR = 0
contMotionOffset = th[0][0] - th_r[0][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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
# calculate jacobian
Jsys, dJsys = controlModel.computeCom_J_dJdq()
JsupL = Jsys[6*supL:6*supL+6, :]
dJsupL = dJsys[6*supL:6*supL+6]
JsupR = Jsys[6*supR:6*supR+6, :]
dJsupR = dJsys[6*supR:6*supR+6]
# calculate contact state
# if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
# contact state
# 0: flying 1: right only 2: left only 3: double
# if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
# elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
# elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
# if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact +=1
# if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact +=2
# initialization
if g_initFlag == 0:
JsysPre = Jsys.copy()
JsupPreL = JsupL.copy()
JsupPreR = JsupR.copy()
JconstPre = Jconst.copy()
softConstPoint = footCenterR.copy()
# yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
# yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
# yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
# footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
# footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact +=1
if refFootL[1] < doubleTosingleOffset:
contact +=2
g_initFlag = 1
# calculate footCenter
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
# if refFootR[1] >doubleTosingleOffset:
# if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
# if contact == 1 or footCenterR[1] > 0.08:
# if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
# elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
if contactChangeCount >0 and contactChangeType == 'StoD':
# change footcenter gradually
footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# todo that, set joint velocities to vpModel
CM_ref_plane = footCenter
# CM_ref_plane[1] += motionModel.getCOM()[1]
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0])/(1/30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2)
dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
if contactChangeCount >0:# and contactChangeType == 'DtoS':
# dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount*10)
dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount)
# dH_des *= (contactChangeCount)/(maxContactChangeCount)*.9+.1
else:
dH_des = None
# H = np.dot(P, np.dot(Jsys, dth_flat))
# dH_des = -Kh* H[3:]
# soft point constraint
#softConstPoint = refFootR.copy()
##softConstPoint[0] += 0.2
#Ksc = 50
#Dsc = 2*(Ksc**.5)
#Bsc = 1.
#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_des - dP_cur)
#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)
#dJconst = (Jconst - Jconst)/(1/30.)
#JconstPre = Jconst.copy()
##yjc.computeJacobianDerivative2(dJconst, DOFs, jointPositions, jointAxeses, linkAngVelocities, [softConstPoint], constJointMasks, False)
#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))
#q_bias1 = np.dot(np.dot(Z, dJconst), dth_flat) + q2
#set up equality constraint
footBodyOriL
L_ddq = mm.logSO3(np.dot(footBodyOriL.T, np.dot(refFootOriL, mm.getSO3FromVectors(np.dot(refFootOriL, mm.unitY()), mm.unitY()))))
R_ddq = mm.logSO3(np.dot(footBodyOriR.T, np.dot(refFootOriR, mm.getSO3FromVectors(np.dot(refFootOriR, mm.unitY()), mm.unitY()))))
L_q = mm.logSO3(footBodyOriL)
R_q = mm.logSO3(footBodyOriR)
L_ang = np.dot(footBodyOriL, footBodyAngVelL)
R_ang = np.dot(footBodyOriR, footBodyAngVelR)
L_dq = mm.vel2qd(L_ang, L_q)
R_dq = mm.vel2qd(R_ang, R_q)
a_oriL = np.dot(footBodyOriL, mm.qdd2accel(L_dq, L_dq, L_q))
a_oriR = np.dot(footBodyOriR, mm.qdd2accel(R_dq, R_dq, R_q))
# body_ddqs = list(map(mm.logSO3, [np.dot(contact_body_ori[i].T, np.dot(ref_body_ori[i], mm.getSO3FromVectors(np.dot(ref_body_ori[i], up_vec_in_each_link[contact_ids[i]]), mm.unitY()))) for i in range(len(contact_body_ori))]))
# body_qs = list(map(mm.logSO3, contact_body_ori))
# body_angs = [np.dot(contact_body_ori[i], contact_body_angvel[i]) for i in range(len(contact_body_ori))]
# body_dqs = [mm.vel2qd(body_angs[i], body_qs[i]) for i in range(len(body_angs))]
# a_oris = [np.dot(contact_body_ori[i], mm.qdd2accel(body_ddqs[i], body_dqs[i], body_qs[i])) for i in range(len(contact_body_ori))]
#
a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0])))
a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0])))
#if contact == 3 and contactChangeCount < maxContactChangeCount/4 and contactChangeCount >=1:
#kt_sup = 30
#viewer.objectInfoWnd.labelSupKt.value(kt_sup)
#viewer.objectInfoWnd.sliderSupKt.value(initSupKt*10)
# a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
# a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) - dt_sup*footBodyVelL, kt_sup*a_oriL-dt_sup*footBodyAngVelL)
a_supL[1] = -kt_sup*footCenterL[1] -dt_sup*footBodyVelL[1]
a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) - dt_sup*footBodyVelR, kt_sup*a_oriR-dt_sup*footBodyAngVelR)
a_supR[1] = -kt_sup*footCenterR[1] -dt_sup*footBodyVelR[1]
##if contact == 2:
#if refFootR[1] <doubleTosingleOffset :
#Jsup = np.vstack((JsupL, JsupR))
#dJsup = np.vstack((dJsupL, dJsupR))
#a_sup = np.append(a_supL, a_supR)
#else:
#Jsup = JsupL.copy()
#dJsup = dJsupL.copy()
#a_sup = a_supL.copy()
# 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)
rs = np.dot(dP, np.dot(Jsys, dth_flat)) + np.dot(P, dJsys)
r_bias, s_bias = np.hsplit(rs, 2)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint2(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint2(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL)
if contactChangeCount >0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
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)
# print(contactForces)
#bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
# extraForce[0] = viewer.objectInfoWnd.labelFm.value() * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1,0,0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0,.1,0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0,0,.1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1,0,0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0,.1,0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0,0,.1]))
rightPos[0] = footCenterL + np.array([.1,0,0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append( contactForces[i]/400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exf_des[0] = [0,0,0]
else:
rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exfen_des[0] = [0,0,0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody) - 0.1 * np.array([viewer.objectInfoWnd.labelForceX.value(), 0., viewer.objectInfoWnd.labelForceZ.value()])
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
# np.set_printoptions(precision=4, linewidth=1000, threshold=np.inf)
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
#motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
frame_step_size = 1. / frame_rate
pydart.init()
dartModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
dartMotionModel = cdm.DartModel(wcfg, motion[0], mcfg, DART_CONTACT_ON)
# wcfg.lockingVel = 0.01
# dartModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
dartModel.translateByOffset(controlToMotionOffset)
totalDOF = dartModel.getTotalDOF()
DOFs = dartModel.getDOFs()
# parameter
Kt = config['Kt']
Dt = config['Dt'] # tracking gain
Kl = config['Kl']
Dl = config['Dl'] # linear balance gain
Kh = config['Kh']
Dh = config['Dh'] # angular balance gain
Ks = config['Ks']
Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
supL = motion[0].skeleton.getJointIndex(config['supLink1'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
# momentum matrix
linkMasses = dartModel.getBodyMasses()
print([body.name for body in dartModel.skeleton.bodynodes])
print(linkMasses)
totalMass = dartModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
#a_sup = (0,0,0, 0,0,0) #ori
#a_sup = (0,0,0, 0,0,0) #L
a_supL = (0, 0, 0, 0, 0, 0)
a_supR = (0, 0, 0, 0, 0, 0)
a_sup_2 = (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
CP_old = [mm.v3(0., 0., 0.)]
CP_des = [None]
dCP_des = [np.zeros(3)]
# penalty method
# bodyIDsToCheck = range(dartModel.getBodyNum())
bodyIDsToCheck = [supL, supR]
#mus = [1.]*len(bodyIDsToCheck)
mus = [.5] * len(bodyIDsToCheck)
# flat data structure
# ddth_des_flat = ype.makeFlatList(totalDOF)
# dth_flat = ype.makeFlatList(totalDOF)
# ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
#viewer.record(False)
viewer.doc.addRenderer(
'motion', yr.JointMotionRenderer(motion, (0, 255, 255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
#viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('motionModel', yr.DartModelRenderer(dartMotionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', yr.DartModelRenderer(dartModel, (255,240,255), yr.POLYGON_FILL))
viewer.doc.addRenderer(
'controlModel',
yr.DartRenderer(dartModel.world, (150, 150, 255), yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_CM_plane',
yr.PointsRenderer(rd_CM_plane, (255, 255, 0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0, 255, 0)))
viewer.doc.addRenderer('rd_CP_des',
yr.PointsRenderer(rd_CP_des, (255, 0, 255)))
viewer.doc.addRenderer(
'rd_dL_des_plane',
yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255, 255, 0)))
viewer.doc.addRenderer('rd_dH_des',
yr.VectorsRenderer(rd_dH_des, rd_CM, (0, 255, 0)))
#viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF',
yr.VectorsRenderer(rd_CF, rd_CF_pos, (255, 0, 0)))
viewer.doc.addRenderer(
'extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos,
(0, 255, 0)))
viewer.doc.addRenderer(
'extraForceEnable',
yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255, 0, 0)))
#viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
#viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
#viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
#viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
#viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
#viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
#success!!
#initKt = 50
#initKl = 10.1
#initKh = 3.1
#initBl = .1
#initBh = .1
#initSupKt = 21.6
#initFm = 100.0
#success!! -- 2015.2.12. double stance
#initKt = 50
#initKl = 37.1
#initKh = 41.8
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 165.0
#single stance
#initKt = 25
#initKl = 80.1
#initKh = 10.8
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 50.0
#single stance -> double stance
#initKt = 25
#initKl = 60.
#initKh = 20.
#initBl = .1
#initBh = .13
#initSupKt = 21.6
#initFm = 50.0
initKt = 25.
# initKl = 11.
# initKh = 22.
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17.
# initSupKt = 2.5
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30 + offset * 9, 40,
20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30 + offset * 9, 40,
20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30 + offset * 9, 40,
20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(
220, 30 + offset * 9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
#self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
pdcontroller = PDController(dartModel, dartModel.skeleton, wcfg.timeStep,
Kt, Dt)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
ik_solver = hikd.numIkSolver(dartMotionModel)
body_num = dartModel.getBodyNum()
# dJsys = np.zeros((6*body_num, totalDOF))
# dJsupL = np.zeros((6, totalDOF))
# dJsupR = np.zeros((6, totalDOF))
# Jpre = [np.zeros((6*body_num, totalDOF)), np.zeros((6, totalDOF)), np.zeros((6, totalDOF))]
###################################
#simulate
###################################
def simulateCallback(frame):
# print()
# print(dartModel.getJointVelocityGlobal(0))
# print(dartModel.getDOFVelocities()[0])
# print(dartModel.get_dq()[:6])
dartMotionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# print('contactstate:', contact, contactChangeCount)
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(
['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2. * (Kt**.5)
Dl = (Kl**.5)
Dh = (Kh**.5)
dt_sup = 2. * (kt_sup**.5)
# Dt = .2*(Kt**.5)
# Dl = .2*(Kl**.5)
# Dh = .2*(Kh**.5)
# dt_sup = .2*(kt_sup**.5)
pdcontroller.setKpKd(Kt, Dt)
footHeight = dartModel.getBody(supL).shapenodes[0].shape.size()[1] / 2.
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
com_offset_x, com_offset_y, com_offset_z = getParamVals(
['com X offset', 'com Y offset', 'com Z offset'])
footOffset = np.array((com_offset_x, com_offset_y, com_offset_z))
des_com = dartMotionModel.getCOM() + footOffset
footCenterL = dartMotionModel.getBodyPositionGlobal(supL)
footCenterR = dartMotionModel.getBodyPositionGlobal(supR)
footBodyOriL = dartMotionModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartMotionModel.getBodyOrientationGlobal(supR)
torso_pos = dartMotionModel.getBodyPositionGlobal(4)
torso_ori = dartMotionModel.getBodyOrientationGlobal(4)
# tracking
# th_r = motion.getDOFPositions(frame)
th_r = dartMotionModel.getDOFPositions()
th = dartModel.getDOFPositions()
th_r_flat = dartMotionModel.get_q()
# dth_r = motion.getDOFVelocities(frame)
# dth = dartModel.getDOFVelocities()
# ddth_r = motion.getDOFAccelerations(frame)
# ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
dth_flat = dartModel.get_dq()
# dth_flat = np.concatenate(dth)
# ddth_des_flat = pdcontroller.compute(dartMotionModel.get_q())
# ddth_des_flat = pdcontroller.compute(th_r)
ddth_des_flat = pdcontroller.compute_flat(th_r_flat)
# ype.flatten(ddth_des, ddth_des_flat)
# ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
footOriL = dartModel.getJointOrientationGlobal(supL)
footOriR = dartModel.getJointOrientationGlobal(supR)
footCenterL = dartModel.getBodyPositionGlobal(supL)
footCenterR = dartModel.getBodyPositionGlobal(supR)
footBodyOriL = dartModel.getBodyOrientationGlobal(supL)
footBodyOriR = dartModel.getBodyOrientationGlobal(supR)
footBodyVelL = dartModel.getBodyVelocityGlobal(supL)
footBodyVelR = dartModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = dartModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = dartModel.getBodyAngVelocityGlobal(supR)
refFootL = dartMotionModel.getBodyPositionGlobal(supL)
refFootR = dartMotionModel.getBodyPositionGlobal(supR)
# refFootAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
# refFootAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootAngVelL = np.zeros(3)
refFootAngVelR = np.zeros(3)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
# refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootVelR = np.zeros(3)
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
# refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
refFootVelL = np.zeros(3)
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1] * frame_step_size + refFootR[
1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1] * frame_step_size + refFootL[
1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
# contMotionOffset = th[0][0] - th_r[0][0]
contMotionOffset = dartModel.getBodyPositionGlobal(
0) - dartMotionModel.getBodyPositionGlobal(0)
linkPositions = dartModel.getBodyPositionsGlobal()
linkVelocities = dartModel.getBodyVelocitiesGlobal()
linkAngVelocities = dartModel.getBodyAngVelocitiesGlobal()
linkInertias = dartModel.getBodyInertiasGlobal()
CM = dartModel.skeleton.com()
dCM = dartModel.skeleton.com_velocity()
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)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL == 1:
contact = 3
maxContactChangeCount += 30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact += 1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact += 2
#initialization
if g_initFlag == 0:
softConstPoint = footCenterR.copy()
footCenter = footCenterL + (footCenterR - footCenterL) / 2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
# if refFootR[1] < doubleTosingleOffset:
# contact +=1
# if refFootL[1] < doubleTosingleOffset:
# contact +=2
if refFootR[1] < footHeight:
contact += 1
if refFootL[1] < footHeight:
contact += 2
g_initFlag = 1
#calculate jacobian
body_num = dartModel.getBodyNum()
Jsys = np.zeros((6 * body_num, totalDOF))
dJsys = np.zeros((6 * body_num, totalDOF))
for i in range(dartModel.getBodyNum()):
# body_i_jacobian = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
# body_i_jacobian_deriv = dartModel.getBody(i).world_jacobian_classic_deriv()[range(-3, 3), :]
# Jsys[6*i:6*i+6, :] = body_i_jacobian
# dJsys[6*i:6*i+6, :] = body_i_jacobian_deriv
Jsys[6 * i:6 * i +
6, :] = dartModel.getBody(i).world_jacobian()[range(-3, 3), :]
dJsys[6 * i:6 * i + 6, :] = dartModel.getBody(
i).world_jacobian_classic_deriv()[range(-3, 3), :]
# dJsys = (Jsys - Jpre[0])/frame_step_size
# Jpre[0] = Jsys.copy()
JsupL = dartModel.getBody(supL).world_jacobian()[range(-3, 3), :]
dJsupL = dartModel.getBody(supL).world_jacobian_classic_deriv()[
range(-3, 3), :]
# dJsupL = np.zeros_like(JsupL)
# dJsupL = (JsupL - Jpre[1])/frame_step_size
# Jpre[1] = JsupL.copy()
JsupR = dartModel.getBody(supR).world_jacobian()[range(-3, 3), :]
dJsupR = dartModel.getBody(supR).world_jacobian_classic_deriv()[
range(-3, 3), :]
# dJsupR = np.zeros_like(JsupR)
# dJsupR = (JsupR - Jpre[2])/frame_step_size
# Jpre[2] = JsupR.copy()
#calculate footCenter
footCenter = .5 * (footCenterL + footCenterR) + footOffset
#if refFootR[1] >doubleTosingleOffset:
#if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
#if contact == 1 or footCenterR[1] > 0.08:
#if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy() + footOffset
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy() + footOffset
footCenter[1] = 0.
if contactChangeCount > 0 and contactChangeType == 'StoD':
#change footcenter gradually
footCenter = preFootCenter + (
maxContactChangeCount - contactChangeCount) * (
footCenter - preFootCenter) / maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
#TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# todo that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = 0.
# CM_ref = footCenter.copy()
# CM_ref[1] = dartMotionModel.getCOM()[1]
# CM_ref += np.array((0., com_offset_y, 0.))
# dL_des_plane = Kl*totalMass*(CM_ref - CM) - Dl*totalMass*dCM
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = [], [], [], []
if DART_CONTACT_ON:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info(
)
else:
bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0]) / frame_step_size
CP_old[0] = CP
CP_des[0] = None
# if CP_des[0] is None:
# CP_des[0] = footCenter
if CP is not None and dCP is not None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des[0] = CP + dCP * frame_step_size + .5 * ddCP_des * (
frame_step_size**2)
dH_des = np.cross(CP_des[0] - CM,
dL_des_plane + totalMass * mm.s2v(wcfg.gravity))
else:
dH_des = None
H = np.dot(P, np.dot(Jsys, dth_flat))
dH_des = -Kh * H[3:]
# set up equality constraint
a_oriL = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0, 1, 0])),
np.array([0, 1, 0])))
a_oriR = mm.logSO3(
mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0, 1, 0])),
np.array([0, 1, 0])))
footErrorL = refFootL.copy()
footErrorL[1] = dartModel.getBody(
supL).shapenodes[0].shape.size()[1] / 2.
footErrorL += -footCenterL + contMotionOffset
footErrorR = refFootR.copy()
footErrorR[1] = dartModel.getBody(
supR).shapenodes[0].shape.size()[1] / 2.
footErrorR += -footCenterR + contMotionOffset
a_supL = np.append(
kt_sup * footErrorL + dt_sup * (refFootVelL - footBodyVelL),
kt_sup * a_oriL + dt_sup * (refFootAngVelL - footBodyAngVelL))
a_supR = np.append(
kt_sup * footErrorR + dt_sup * (refFootVelR - footBodyVelR),
kt_sup * a_oriR + dt_sup * (refFootAngVelR - footBodyAngVelR))
# 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)
#######################################################
# optimization
#######################################################
#if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
#if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton,
config['weightMap'])
print(w)
#if contact == 2:
#mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
if dH_des is not None:
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
#mot.setConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#mot.addConstraint(problem, totalDOF, Jsup, dJsup, dth_flat, a_sup)
#if contact & 1 and contactChangeCount == 0:
if contact & 1:
#if refFootR[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat,
a_supR)
if contact & 2:
#if refFootL[1] < doubleTosingleOffset:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat,
a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
# ype.nested(r['x'], ddth_sol)
ddth_sol = np.asarray(r['x'])
# ddth_sol[:6] = np.zeros(6)
rootPos[0] = dartModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
inv_h = 1. / dartModel.world.time_step()
for i in range(stepsPerFrame):
ddq, tau, bodyIDs, contactPositions, contactPositionLocals, contactForces = hqp.calc_QP(
dartModel.skeleton, ddth_sol, inv_h)
# ddq, tau, bodyIDs, contactPositions, contactPositionLocals, contactForces = hqp.calc_QP(dartModel.skeleton, ddth_des_flat, inv_h)
# print(frame, i, tau)
dartModel.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
dartModel.skeleton.set_forces(tau)
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([
viewer.objectInfoWnd.labelForceX.value(),
viewer.objectInfoWnd.labelForceY.value(),
viewer.objectInfoWnd.labelForceZ.value()
])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
dartModel.applyPenaltyForce(selectedBodyId, localPos,
extraForce)
dartModel.step()
# if DART_CONTACT_ON:
# bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.get_dart_contact_info()
# else:
# bodyIDs, contactPositions, contactPositionLocals, contactForces = dartModel.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des[0]
rd_dL_des_plane[0] = [
dL_des_plane[0] / 100, dL_des_plane[1] / 100,
dL_des_plane[2] / 100
]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass * mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i] / 100)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [
extraForce[0][0] / 100, extraForce[0][1] / 100,
extraForce[0][2] / 100
]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = dartModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1 / 30.)
viewer.show()
Fl.run()
def main():
np.set_printoptions(precision=4, linewidth=200)
motion, mcfg, wcfg, stepsPerFrame, config, frame_rate = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_jump_biped()
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
#controlToMotionOffset = (1.5, -0.02, 0)
controlToMotionOffset = (1.5, 0, 0)
controlModel.translateByOffset(controlToMotionOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
foot_dofs = []
left_foot_dofs = []
right_foot_dofs = []
foot_seg_dofs = []
left_foot_seg_dofs = []
right_foot_seg_dofs = []
# for joint_idx in range(motion[0].skeleton.getJointNum()):
for joint_idx in range(controlModel.getJointNum()):
joint_name = controlModel.index2name(joint_idx)
print(joint_idx, joint_name)
# joint_name = motion[0].skeleton.getJointName(joint_idx)
if 'Foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
if 'foot' in joint_name:
foot_dofs_temp = controlModel.getJointDOFIndexes(joint_idx)
foot_seg_dofs.extend(foot_dofs_temp)
if 'Left' in joint_name:
left_foot_dofs.extend(foot_dofs_temp)
elif 'Right' in joint_name:
right_foot_dofs.extend(foot_dofs_temp)
# parameter
Kt = config['Kt']; Dt = config['Dt'] # tracking gain
Kl = config['Kl']; Dl = config['Dl'] # linear balance gain
Kh = config['Kh']; Dh = config['Dh'] # angular balance gain
Ks = config['Ks']; Ds = config['Ds'] # penalty force spring gain
Bt = config['Bt']
Bl = config['Bl']
Bh = config['Bh']
supL = motion[0].skeleton.getJointIndex(config['supLink1'])
supR = motion[0].skeleton.getJointIndex(config['supLink2'])
selectedBody = motion[0].skeleton.getJointIndex(config['end'])
constBody = motion[0].skeleton.getJointIndex('RightFoot')
# jacobian
JsupL = yjc.makeEmptyJacobian(DOFs, 1)
dJsupL = JsupL.copy()
JsupPreL = JsupL.copy()
JsupR = yjc.makeEmptyJacobian(DOFs, 1)
dJsupR = JsupR.copy()
JsupPreR = JsupR.copy()
Jconst = yjc.makeEmptyJacobian(DOFs, 1)
dJconst = Jconst.copy()
JconstPre = Jconst.copy()
Jsys = yjc.makeEmptyJacobian(DOFs, controlModel.getBodyNum())
dJsys = Jsys.copy()
JsysPre = Jsys.copy()
supLJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supL)]
supRJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, supR)]
constJointMasks = [yjc.getLinkJointMask(motion[0].skeleton, constBody)]
allLinkJointMasks = yjc.getAllLinkJointMasks(motion[0].skeleton)
# momentum matrix
linkMasses = controlModel.getBodyMasses()
totalMass = controlModel.getTotalMass()
TO = ymt.make_TO(linkMasses)
dTO = ymt.make_dTO(len(linkMasses))
# optimization
problem = yac.LSE(totalDOF, 12)
# a_sup = (0,0,0, 0,0,0) #ori
# a_sup = (0,0,0, 0,0,0) #L
a_supL = (0,0,0, 0,0,0)
a_supR = (0,0,0, 0,0,0)
a_sup_2 = (0,0,0, 0,0,0, 0,0,0, 0,0,0)
CP_old = [mm.v3(0.,0.,0.)]
# penalty method
bodyIDsToCheck = range(vpWorld.getBodyNum())
# mus = [1.]*len(bodyIDsToCheck)
mus = [.5]*len(bodyIDsToCheck)
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
# viewer
rd_footCenter = [None]
rd_footCenterL = [None]
rd_footCenterR = [None]
rd_CM_plane = [None]
rd_CM = [None]
rd_CP = [None]
rd_CP_des = [None]
rd_dL_des_plane = [None]
rd_dH_des = [None]
rd_grf_des = [None]
rd_exf_des = [None]
rd_exfen_des = [None]
rd_root_des = [None]
rd_CF = [None]
rd_CF_pos = [None]
rootPos = [None]
selectedBodyId = [selectedBody]
extraForce = [None]
extraForcePos = [None]
rightFootVectorX = [None]
rightFootVectorY = [None]
rightFootVectorZ = [None]
rightFootPos = [None]
rightVectorX = [None]
rightVectorY = [None]
rightVectorZ = [None]
rightPos = [None]
# viewer = ysv.SimpleViewer()
viewer = hsv.hpSimpleViewer(viewForceWnd=False)
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,255,255), yr.LINK_BONE))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motionModel', yr.VpModelRenderer(motionModel, (150,150,255), yr.POLYGON_FILL))
# viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
control_model_renderer = yr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_FILL)
viewer.doc.addRenderer('controlModel', control_model_renderer)
control_model_renderer.body_colors[supL] = (255, 0, 0)
control_model_renderer.body_colors[supR] = (255, 0, 0)
viewer.doc.addRenderer('rd_footCenter', yr.PointsRenderer(rd_footCenter))
viewer.doc.addRenderer('rd_CM_plane', yr.PointsRenderer(rd_CM_plane, (255,255,0)))
viewer.doc.addRenderer('rd_CP', yr.PointsRenderer(rd_CP, (0,255,0)))
viewer.doc.addRenderer('rd_CP_des', yr.PointsRenderer(rd_CP_des, (255,0,255)))
viewer.doc.addRenderer('rd_dL_des_plane', yr.VectorsRenderer(rd_dL_des_plane, rd_CM, (255,255,0)))
viewer.doc.addRenderer('rd_dH_des', yr.VectorsRenderer(rd_dH_des, rd_CM, (0,255,0)))
# viewer.doc.addRenderer('rd_grf_des', yr.ForcesRenderer(rd_grf_des, rd_CP_des, (0,255,0), .001))
viewer.doc.addRenderer('rd_CF', yr.VectorsRenderer(rd_CF, rd_CF_pos, (255,255,0)))
viewer.doc.addRenderer('extraForce', yr.VectorsRenderer(rd_exf_des, extraForcePos, (0,255,0)))
viewer.doc.addRenderer('extraForceEnable', yr.VectorsRenderer(rd_exfen_des, extraForcePos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriX', yr.VectorsRenderer(rightFootVectorX, rightFootPos, (255,0,0)))
# viewer.doc.addRenderer('right_foot_oriY', yr.VectorsRenderer(rightFootVectorY, rightFootPos, (0,255,0)))
# viewer.doc.addRenderer('right_foot_oriZ', yr.VectorsRenderer(rightFootVectorZ, rightFootPos, (0,0,255)))
# viewer.doc.addRenderer('right_oriX', yr.VectorsRenderer(rightVectorX, rightPos, (255,0,0)))
# viewer.doc.addRenderer('right_oriY', yr.VectorsRenderer(rightVectorY, rightPos, (0,255,0)))
# viewer.doc.addRenderer('right_oriZ', yr.VectorsRenderer(rightVectorZ, rightPos, (0,0,255)))
# success!!
# initKt = 50
# initKl = 10.1
# initKh = 3.1
# initBl = .1
# initBh = .1
# initSupKt = 21.6
# initFm = 100.0
# success!! -- 2015.2.12. double stance
# initKt = 50
# initKl = 37.1
# initKh = 41.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 165.0
# single stance
# initKt = 25
# initKl = 80.1
# initKh = 10.8
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
# single stance -> double stance
# initKt = 25
# initKl = 60.
# initKh = 20.
# initBl = .1
# initBh = .13
# initSupKt = 21.6
# initFm = 50.0
initKt = 25
initKl = 100.
initKh = 100.
initBl = .1
initBh = .13
initSupKt = 17
initFm = 50.0
initComX = 0.
initComY = 0.
initComZ = 0.
viewer.objectInfoWnd.add1DSlider("Kt", 0., 300., 1., initKt)
viewer.objectInfoWnd.add1DSlider("Kl", 0., 300., 1., initKl)
viewer.objectInfoWnd.add1DSlider("Kh", 0., 300., 1., initKh)
viewer.objectInfoWnd.add1DSlider("Bl", 0., 1., .001, initBl)
viewer.objectInfoWnd.add1DSlider("Bh", 0., 1., .001, initBh)
viewer.objectInfoWnd.add1DSlider("SupKt", 0., 100., 0.1, initSupKt)
viewer.objectInfoWnd.add1DSlider("Fm", 0., 1000., 10., initFm)
viewer.objectInfoWnd.add1DSlider("com X offset", -1., 1., 0.01, initComX)
viewer.objectInfoWnd.add1DSlider("com Y offset", -1., 1., 0.01, initComY)
viewer.objectInfoWnd.add1DSlider("com Z offset", -1., 1., 0.01, initComZ)
viewer.force_on = False
def viewer_SetForceState(object):
viewer.force_on = True
def viewer_GetForceState():
return viewer.force_on
def viewer_ResetForceState():
viewer.force_on = False
viewer.objectInfoWnd.addBtn('Force on', viewer_SetForceState)
viewer_ResetForceState()
offset = 60
viewer.objectInfoWnd.begin()
viewer.objectInfoWnd.labelForceX = Fl_Value_Input(20, 30+offset*9, 40, 20, 'X')
viewer.objectInfoWnd.labelForceX.value(0)
viewer.objectInfoWnd.labelForceY = Fl_Value_Input(80, 30+offset*9, 40, 20, 'Y')
viewer.objectInfoWnd.labelForceY.value(0)
viewer.objectInfoWnd.labelForceZ = Fl_Value_Input(140, 30+offset*9, 40, 20, 'Z')
viewer.objectInfoWnd.labelForceZ.value(1)
viewer.objectInfoWnd.labelForceDur = Fl_Value_Input(220, 30+offset*9, 40, 20, 'Dur')
viewer.objectInfoWnd.labelForceDur.value(0.1)
viewer.objectInfoWnd.end()
# self.sliderFm = Fl_Hor_Nice_Slider(10, 42+offset*6, 250, 10)
def getParamVal(paramname):
return viewer.objectInfoWnd.getVal(paramname)
def getParamVals(paramnames):
return (getParamVal(name) for name in paramnames)
extendedFootName = ['Foot_foot_0_0', 'Foot_foot_0_1', 'Foot_foot_0_0_0', 'Foot_foot_0_1_0', 'Foot_foot_1_0']
lIDdic = {'Left'+name:motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName}
rIDdic = {'Right'+name:motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName}
footIdDic = lIDdic.copy()
footIdDic.update(rIDdic)
lIDlist = [motion[0].skeleton.getJointIndex('Left'+name) for name in extendedFootName]
rIDlist = [motion[0].skeleton.getJointIndex('Right'+name) for name in extendedFootName]
footIdlist = []
footIdlist.extend(lIDlist)
footIdlist.extend(rIDlist)
foot_left_idx = motion[0].skeleton.getJointIndex('LeftFoot')
foot_right_idx = motion[0].skeleton.getJointIndex('RightFoot')
foot_left_idx_temp = motion[0].skeleton.getJointIndex('LeftFoot_foot_1_0')
foot_right_idx_temp = motion[0].skeleton.getJointIndex('RightFoot_foot_1_0')
def get_jacobianbase_and_masks(skeleton, DOFs, joint_idx):
J = yjc.makeEmptyJacobian(DOFs, 1)
joint_masks = [yjc.getLinkJointMask(skeleton, joint_idx)]
return J, joint_masks
###################################
# simulate
###################################
def simulateCallback(frame):
motionModel.update(motion[frame])
global g_initFlag
global forceShowTime
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global JconstPre
global preFootCenter
global maxContactChangeCount
global contactChangeCount
global contact
global contactChangeType
# Kt, Kl, Kh, Bl, Bh, kt_sup = viewer.GetParam()
Kt, Kl, Kh, Bl, Bh, kt_sup = getParamVals(['Kt', 'Kl', 'Kh', 'Bl', 'Bh', 'SupKt'])
Dt = 2*(Kt**.5)
Dl = 2*(Kl**.5)
Dh = 2*(Kh**.5)
dt_sup = 2*(kt_sup**.5)
doubleTosingleOffset = 0.15
singleTodoubleOffset = 0.30
#doubleTosingleOffset = 0.09
doubleTosingleVelOffset = 0.0
# 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)
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
#################################################
# jacobian
#################################################
# caution!! body orientation and joint orientation of foot are totally different!!
footOriL = controlModel.getJointOrientationGlobal(supL)
footOriR = controlModel.getJointOrientationGlobal(supR)
# desire footCenter[1] = 0.041135
# desire footCenter[1] = 0.0197
footCenterL = controlModel.getBodyPositionGlobal(supL)
footCenterR = controlModel.getBodyPositionGlobal(supR)
footBodyOriL = controlModel.getBodyOrientationGlobal(supL)
footBodyOriR = controlModel.getBodyOrientationGlobal(supR)
footBodyVelL = controlModel.getBodyVelocityGlobal(supL)
footBodyVelR = controlModel.getBodyVelocityGlobal(supR)
footBodyAngVelL = controlModel.getBodyAngVelocityGlobal(supL)
footBodyAngVelR = controlModel.getBodyAngVelocityGlobal(supR)
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
refFootVelL = motionModel.getBodyVelocityGlobal(supL)
refFootVelR = motionModel.getBodyVelocityGlobal(supR)
refFootAngVelL = motionModel.getBodyAngVelocityGlobal(supL)
refFootAngVelR = motionModel.getBodyAngVelocityGlobal(supR)
refFootJointVelR = motion.getJointVelocityGlobal(supR, frame)
refFootJointAngVelR = motion.getJointAngVelocityGlobal(supR, frame)
refFootJointR = motion.getJointPositionGlobal(supR, frame)
refFootVelR = refFootJointVelR + np.cross(refFootJointAngVelR, (refFootR-refFootJointR))
refFootJointVelL = motion.getJointVelocityGlobal(supL, frame)
refFootJointAngVelL = motion.getJointAngVelocityGlobal(supL, frame)
refFootJointL = motion.getJointPositionGlobal(supL, frame)
refFootVelL = refFootJointVelL + np.cross(refFootJointAngVelL, (refFootL-refFootJointL))
contactR = 1
contactL = 1
if refFootVelR[1] < 0 and refFootVelR[1]/30. + refFootR[1] > singleTodoubleOffset:
contactR = 0
if refFootVelL[1] < 0 and refFootVelL[1]/30. + refFootL[1] > singleTodoubleOffset:
contactL = 0
if refFootVelR[1] > 0 and refFootVelR[1]/30. + refFootR[1] > doubleTosingleOffset:
contactR = 0
if refFootVelL[1] > 0 and refFootVelL[1]/30. + refFootL[1] > doubleTosingleOffset:
contactL = 0
# contactR = 0
contMotionOffset = th[0][0] - th_r[0][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.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM, linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses, linkVelocities, dCM, linkAngVelocities, linkInertias)
#calculate contact state
#if g_initFlag == 1 and contact == 1 and refFootR[1] < doubleTosingleOffset and footCenterR[1] < 0.08:
if g_initFlag == 1:
#contact state
# 0: flying 1: right only 2: left only 3: double
#if contact == 2 and refFootR[1] < doubleTosingleOffset:
if contact == 2 and contactR==1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootL[1] < doubleTosingleOffset:
elif contact == 1 and contactL==1:
contact = 3
maxContactChangeCount+=30
contactChangeCount += maxContactChangeCount
contactChangeType = 'StoD'
#elif contact == 3 and refFootR[1] > doubleTosingleOffset:
elif contact == 3 and contactR == 0:
contact = 2
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
#elif contact == 3 and refFootL[1] > doubleTosingleOffset:
elif contact == 3 and contactL == 0:
contact = 1
contactChangeCount += maxContactChangeCount
contactChangeType = 'DtoS'
else:
contact = 0
#if refFootR[1] < doubleTosingleOffset:
if contactR == 1:
contact +=1
#if refFootL[1] < doubleTosingleOffset:
if contactL == 1:
contact +=2
#initialization
if g_initFlag == 0:
JsysPre = Jsys.copy()
# JsupPreL = JsupL.copy()
# JsupPreR = JsupR.copy()
JconstPre = Jconst.copy()
softConstPoint = footCenterR.copy()
yjc.computeJacobian2(JsysPre, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# yjc.computeJacobian2(JsupPreL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
# yjc.computeJacobian2(JsupPreR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
yjc.computeJacobian2(JconstPre, DOFs, jointPositions, jointAxeses, [softConstPoint], constJointMasks)
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
footCenter[1] = 0.
preFootCenter = footCenter.copy()
#footToBodyFootRotL = np.dot(np.transpose(footOriL), footBodyOriL)
#footToBodyFootRotR = np.dot(np.transpose(footOriR), footBodyOriR)
if refFootR[1] < doubleTosingleOffset:
contact += 1
if refFootL[1] < doubleTosingleOffset:
contact += 2
g_initFlag = 1
# calculate jacobian
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses, linkPositions, allLinkJointMasks)
# dJsys = (Jsys - JsysPre)/(1/30.)
# JsysPre = Jsys.copy()
yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
JsupL, supLJointMasks = get_jacobianbase_and_masks(motion[0].skeleton, DOFs, supL)
yjc.computeJacobian2(JsupL, DOFs, jointPositions, jointAxeses, [footCenterL], supLJointMasks)
# dJsupL = (JsupL - JsupPreL)/(1/30.)
# JsupPreL = JsupL.copy()
yjc.computeJacobianDerivative2(dJsupL, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterL], supLJointMasks)
JsupR, supRJointMasks = get_jacobianbase_and_masks(motion[0].skeleton, DOFs, supR)
yjc.computeJacobian2(JsupR, DOFs, jointPositions, jointAxeses, [footCenterR], supRJointMasks)
# dJsupR = (JsupR - JsupPreR)/(1/30.)
# JsupPreR = JsupR.copy()
yjc.computeJacobianDerivative2(dJsupR, DOFs, jointPositions, jointAxeses, linkAngVelocities, [footCenterR], supRJointMasks)
# calculate footCenter
footCenter = footCenterL + (footCenterR - footCenterL)/2.0
#if refFootR[1] >doubleTosingleOffset:
#if refFootR[1] > doubleTosingleOffset or footCenterR[1] > 0.08:
#if contact == 1 or footCenterR[1] > 0.08:
#if contact == 2 or footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
footCenter = footCenterL.copy()
#elif contact == 1 or footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
footCenter = footCenterR.copy()
footCenter[1] = 0.
if contactChangeCount >0 and contactChangeType == 'StoD':
#change footcenter gradually
footCenter = preFootCenter + (maxContactChangeCount - contactChangeCount)*(footCenter-preFootCenter)/maxContactChangeCount
preFootCenter = footCenter.copy()
# linear momentum
# TODO:
# We should consider dCM_ref, shouldn't we?
# add getBodyPositionGlobal and getBodyPositionsGlobal in csVpModel!
# to do that, set joint velocities to vpModel
CM_ref_plane = footCenter
dL_des_plane = Kl*totalMass*(CM_ref_plane - CM_plane) - Dl*totalMass*dCM_plane
# dL_des_plane[1] = 0.
# angular momentum
CP_ref = footCenter
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
if CP_old[0] is None or CP is None:
dCP = None
else:
dCP = (CP - CP_old[0])/(1/30.)
CP_old[0] = CP
if CP is not None and dCP is not None:
ddCP_des = Kh*(CP_ref - CP) - Dh*(dCP)
CP_des = CP + dCP*(1/30.) + .5*ddCP_des*((1/30.)**2)
dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
if contactChangeCount > 0: # and contactChangeType == 'DtoS':
dH_des *= (maxContactChangeCount - contactChangeCount)/(maxContactChangeCount)
else:
dH_des = None
# set up equality constraint
# left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(motion[frame], footIdDic, None)
left_foot_up_vec, right_foot_up_vec = hfi.get_foot_up_vector(controlModel, footIdDic, None)
# print(left_foot_up_vec, right_foot_up_vec)
# a_oriL = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriL, np.array([0,1,0])), np.array([0,1,0])))
# a_oriR = mm.logSO3(mm.getSO3FromVectors(np.dot(footBodyOriR, np.array([0,1,0])), np.array([0,1,0])))
a_oriL = mm.logSO3(mm.getSO3FromVectors(left_foot_up_vec, np.array([0,1,0])))
a_oriR = mm.logSO3(mm.getSO3FromVectors(right_foot_up_vec, np.array([0,1,0])))
a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), kt_sup*a_oriL+dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), kt_sup*a_oriR+dt_sup*(refFootAngVelR-footBodyAngVelR))
if contactChangeCount > 0 and contactChangeType == 'DtoS':
a_supL = np.append(kt_sup*(refFootL - footCenterL + contMotionOffset) + dt_sup*(refFootVelL - footBodyVelL), 4*kt_sup*a_oriL+2*dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(kt_sup*(refFootR - footCenterR + contMotionOffset) + dt_sup*(refFootVelR - footBodyVelR), 4*kt_sup*a_oriR+2*dt_sup*(refFootAngVelR-footBodyAngVelR))
elif contactChangeCount > 0 and contactChangeType == 'StoD':
linkt = (13.*contactChangeCount)/(maxContactChangeCount)+1.
lindt = 2*(linkt**.5)
angkt = (13.*contactChangeCount)/(maxContactChangeCount)+1.
angdt = 2*(angkt**.5)
a_supL = np.append(linkt*kt_sup*(refFootL - footCenterL + contMotionOffset) + lindt*dt_sup*(refFootVelL - footBodyVelL), angkt*kt_sup*a_oriL+angdt*dt_sup*(refFootAngVelL-footBodyAngVelL))
a_supR = np.append(linkt*kt_sup*(refFootR - footCenterR + contMotionOffset) + lindt*dt_sup*(refFootVelR - footBodyVelR), angkt*kt_sup*a_oriR+angdt*dt_sup*(refFootAngVelR-footBodyAngVelR))
# 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)
#######################################################
# optimization
#######################################################
# if contact == 2 and footCenterR[1] > doubleTosingleOffset/2:
if contact == 2:
config['weightMap']['RightUpLeg'] = .8
config['weightMap']['RightLeg'] = .8
config['weightMap']['RightFoot'] = .8
else:
config['weightMap']['RightUpLeg'] = .1
config['weightMap']['RightLeg'] = .25
config['weightMap']['RightFoot'] = .2
# if contact == 1 and footCenterL[1] > doubleTosingleOffset/2:
if contact == 1:
config['weightMap']['LeftUpLeg'] = .8
config['weightMap']['LeftLeg'] = .8
config['weightMap']['LeftFoot'] = .8
else:
config['weightMap']['LeftUpLeg'] = .1
config['weightMap']['LeftLeg'] = .25
config['weightMap']['LeftFoot'] = .2
w = mot.getTrackingWeight(DOFs, motion[0].skeleton, config['weightMap'])
# if contact == 2:
# mot.addSoftPointConstraintTerms(problem, totalDOF, Bsc, ddP_des1, Q1, q_bias1)
mot.addTrackingTerms(problem, totalDOF, Bt, w, ddth_des_flat)
if dH_des is not None:
mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
if contact & 1:
mot.addConstraint(problem, totalDOF, JsupR, dJsupR, dth_flat, a_supR)
if contact & 2:
mot.addConstraint(problem, totalDOF, JsupL, dJsupL, dth_flat, a_supL)
if contactChangeCount > 0:
contactChangeCount -= 1
if contactChangeCount == 0:
maxContactChangeCount = 30
contactChangeType = 0
r = problem.solve()
problem.clear()
ddth_sol_flat = np.asarray(r['x'])
# ddth_sol_flat[foot_seg_dofs] = np.array(ddth_des_flat)[foot_seg_dofs]
print(foot_seg_dofs)
ype.nested(ddth_sol_flat, 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)
# bodyIDs, contactPositions, contactPositionLocals, contactForces, contactVelocities = vpWorld.calcManyPenaltyForce(0, bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
if forceShowTime > viewer.objectInfoWnd.labelForceDur.value():
forceShowTime = 0
viewer_ResetForceState()
forceforce = np.array([viewer.objectInfoWnd.labelForceX.value(), viewer.objectInfoWnd.labelForceY.value(), viewer.objectInfoWnd.labelForceZ.value()])
extraForce[0] = getParamVal('Fm') * mm.normalize2(forceforce)
if viewer_GetForceState():
forceShowTime += wcfg.timeStep
vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
vpWorld.step()
# rendering
rightFootVectorX[0] = np.dot(footOriL, np.array([.1, 0, 0]))
rightFootVectorY[0] = np.dot(footOriL, np.array([0, .1, 0]))
rightFootVectorZ[0] = np.dot(footOriL, np.array([0, 0, .1]))
rightFootPos[0] = footCenterL
rightVectorX[0] = np.dot(footBodyOriL, np.array([.1, 0, 0]))
rightVectorY[0] = np.dot(footBodyOriL, np.array([0, .1, 0]))
rightVectorZ[0] = np.dot(footBodyOriL, np.array([0, 0, .1]))
rightPos[0] = footCenterL + np.array([.1, 0, 0])
rd_footCenter[0] = footCenter
rd_footCenterL[0] = footCenterL
rd_footCenterR[0] = footCenterR
rd_CM[0] = CM
rd_CM_plane[0] = CM.copy()
rd_CM_plane[0][1] = 0.
if CP is not None and dCP is not None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = [dL_des_plane[0]/100, dL_des_plane[1]/100, dL_des_plane[2]/100]
rd_dH_des[0] = dH_des
rd_grf_des[0] = dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_root_des[0] = rootPos[0]
del rd_CF[:]
del rd_CF_pos[:]
for i in range(len(contactPositions)):
rd_CF.append(contactForces[i]/400)
rd_CF_pos.append(contactPositions[i].copy())
if viewer_GetForceState():
rd_exfen_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exf_des[0] = [0, 0, 0]
else:
rd_exf_des[0] = [extraForce[0][0]/100, extraForce[0][1]/100, extraForce[0][2]/100]
rd_exfen_des[0] = [0, 0, 0]
extraForcePos[0] = controlModel.getBodyPositionGlobal(selectedBody)
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def create_jump_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
#motionName = 'wd2_jump.bvh'
#motionName = 'wd2_stand.bvh'
motionName = 'woddy2_jump_ori.bvh'
motion = yf.readBvhFile(motionName, .01)
#yme.removeJoint(motion, 'Head', False)
yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.6),
False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.6),
False)
yme.updateGlobalT(motion)
motion.translateByOffset((0, -0.05, 0))
#motion = motion[130:]
#motion = motion[120:]
motion = motion[67:162]
#motion = motion[-249:-248]
motion[0:0] = [motion[0]] * 200
motion.extend([motion[-1]] * 500)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .25
node = mcfg.getNode('RightFoot')
node.length = .25
node.width = .15
node.mass = 2.
#node.width = .2
#node.mass = 1.
node = mcfg.getNode('LeftFoot')
node.length = .25
node.width = .15
node.mass = 2.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
#stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#wcfg.timeStep = (1/1000.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':1., 'Spine1':1., 'RightFoot':.5, 'LeftFoot':.5, 'Hips':1.,\
'RightUpLeg':.5, 'RightLeg':.5, 'LeftUpLeg':.5, 'LeftLeg':.5}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1., 'Spine1':1., 'RightFoot':1.0, 'LeftFoot':1.0, 'Hips':1.5,\
#'RightUpLeg':2., 'RightLeg':2., 'LeftUpLeg':2., 'LeftLeg':2.}
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':1.,\
#'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
#success!!
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.5, 'Spine1':.5, 'RightFoot':1., 'LeftFoot':1., 'Hips':0.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1., 'LeftLeg':1.}
'''
#config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
#'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
#'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink1'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_any_motion(motion):
#motion
yme.removeJoint(motion, 'Head', False)
#yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03, -.05, 0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1, 0, 0), .01), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.3), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.3), -.5), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1, -0.5, 0), -.45),
False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1, 0.5, 0), -.45),
False)
yme.updateGlobalT(motion)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
#node.length = .2
#node.width = .12
#node.width = .2
node.width = .15
node.mass = 2.
#node.mass = 1.
node = mcfg.getNode('LeftFoot')
node.length = .25
#node.length = .2
#node.width = .12
node.width = .15
#node.width = .2
node.mass = 2.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 60
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.6, 'Spine1':.6, 'RightFoot':.2, 'LeftFoot':.2, 'Hips':0.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
return mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
#motionName = 'wd2_jump_ori.bvh'
if 1:
#motionName = 'wd2_stand.bvh'
motionName = 'woddy2_jump0.bvh'
motion = yf.readBvhFile(motionName, .01)
motion.translateByOffset((0., -0.023, 0.))
yme.removeJoint(motion, 'HEad', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder1', False)
yme.removeJoint(motion, 'RightToes_Effector', False)
yme.removeJoint(motion, 'LeftToes_Effector', False)
yme.removeJoint(motion, 'RightHand_Effector', False)
yme.removeJoint(motion, 'LeftHand_Effector', False)
yme.offsetJointLocal(motion, 'RightArm', (.03,-.05,0), False)
yme.offsetJointLocal(motion, 'LeftArm', (-.03,-.05,0), False)
yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(2.5,-0.0,.3), -.5), False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(2.5,0.0,-.3), -.5), False)
#yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(1,-0.0,.2), -.5), False)
#yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1,0.0,-.2), -.5), False)
yme.rotateJointLocal(motion, 'LeftUpLeg', mm.exp(mm.v3(0.0,.0,1.), .08), False)
yme.rotateJointLocal(motion, 'LeftLeg', mm.exp(mm.v3(0.0,1.0,0.), -.2), False)
#yme.rotateJointLocal(motion, 'RightLeg', mm.exp(mm.v3(1.0,0.0,0.), -.1), False)
yme.updateGlobalT(motion)
# yf.writeBvhFile('wd2_jump0.bvh', motion)
else :
motionName = 'ww13_41.bvh'
motion = yf.readBvhFile(motionName, 0.056444)
yme.removeJoint(motion, 'LHipJoint', False)
yme.removeJoint(motion, 'RHipJoint', False)
yme.removeJoint(motion, 'Neck', False)
yme.removeJoint(motion, 'Neck1', False)
yme.removeJoint(motion, 'Head', False)
yme.removeJoint(motion, 'RightShoulder', False)
yme.removeJoint(motion, 'LeftShoulder', False)
yme.removeJoint(motion, 'RightToeBase_Effector', False)
yme.removeJoint(motion, 'LeftToeBase_Effector', False)
yme.removeJoint(motion, 'LeftHand', False)
yme.removeJoint(motion, 'LeftFingerBase', False)
yme.removeJoint(motion, 'LeftHandIndex1_Effector', False)
yme.removeJoint(motion, 'LThumb', False)
yme.removeJoint(motion, 'RightHand', False)
yme.removeJoint(motion, 'RightFingerBase', False)
yme.removeJoint(motion, 'RightHandIndex1_Effector', False)
yme.removeJoint(motion, 'RThumb', False)
yme.removeJoint(motion, 'LowerBack', False)
yme.offsetJointLocal(motion, 'RightArm', (-.03,-.05,0), False)
yme.offsetJointLocal(motion, 'LeftArm', (.03,-.05,0), False)
#yme.rotateJointLocal(motion, 'Hips', mm.exp(mm.v3(1,0,0), .01), False)
yme.rotateJointLocal(motion, 'LeftFoot', mm.exp(mm.v3(-1.5,0,1), .4), False)
yme.rotateJointLocal(motion, 'RightFoot', mm.exp(mm.v3(1.5,0,1), -.4), False)
yme.updateGlobalT(motion)
#motion = motion[50:-1]
motion = motion[240:-1]
#motion = motion[40:-58]
#motion = motion[56:-248]
#motion = motion[-249:-248]
#motion = motion[62:65]
#motion = motion[216:217]
#motion = motion[515:555]
################
motion = motion[515:555]
# motion = motion[164:280]
#motion = motion[96:97]
motion[0:0] = [motion[0]]*100
motion.extend([motion[-1]]*5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode('Hips')
node.length = .2
node.width = .25
node = mcfg.getNode('Spine1')
node.length = .2
node.offset = (0,0,0.1)
node = mcfg.getNode('Spine')
node.width = .22
#node.length = .2 ####
node = mcfg.getNode('RightFoot')
node.length = .25
node.width = .2
node.mass = 4.
node = mcfg.getNode('LeftFoot')
node.length = .25
node.width = .2
node.mass = 4.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/(stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = 2.5; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 2.5
config['Bh'] = 1.
'''
config['Kt'] = 200; config['Dt'] = 2*(config['Kt']**.5) # tracking gain
config['Kl'] = .10; config['Dl'] = 2*(config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1; config['Dh'] = 2*(config['Kh']**.5) # angular balance gain
config['Ks'] = 20000; config['Ds'] = 2*(config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1.#0.5
config['Bh'] = 1.
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':.3, 'Spine1':.3, 'RightFoot':.3, 'LeftFoot':.3, 'Hips':.5,\
'RightUpLeg':.1, 'RightLeg':.3, 'LeftUpLeg':.1, 'LeftLeg':.3}
'''
config['IKweightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':0.5, 'Spine1':0.5, 'RightFoot':1.2, 'LeftFoot':1.2, 'Hips':1.2,\
'RightUpLeg':.9, 'RightLeg':.9, 'LeftUpLeg':.9, 'LeftLeg':.9}
'''
config['weightMap']={'RightArm':.2, 'RightForeArm':.2, 'LeftArm':.2, 'LeftForeArm':.2,\
'Spine':1.5, 'LeftFoot':1., 'Hips':1.5,\
'RightUpLeg':1., 'RightLeg':1., 'LeftUpLeg':1.5, 'LeftLeg':1.5}
config['supLink'] = 'LeftFoot'
config['supLink2'] = 'RightFoot'
#config['end'] = 'Hips'
config['end'] = 'Spine1'
return motion, mcfg, wcfg, stepsPerFrame, config
def create_biped():
# motion
#motionName = 'wd2_n_kick.bvh'
if MOTION == STAND:
motionName = 'wd2_stand.bvh'
elif MOTION == STAND2:
motionName = 'ww13_41_V001.bvh'
elif MOTION == FORWARD_JUMP:
motionName = 'woddy2_jump0.bvh'
elif MOTION == TAEKWONDO:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == TAEKWONDO2:
motionName = './MotionFile/wd2_098_V001.bvh'
elif MOTION == KICK:
motionName = 'wd2_n_kick.bvh'
elif MOTION == WALK:
motionName = 'wd2_WalkForwardNormal00.bvh'
elif MOTION == TIPTOE:
motionName = './MotionFile/cmu/15_07_15_07.bvh'
#motionName = 'ww13_41_V001.bvh'
scale = 0.01
if MOTION == WALK:
scale = 1.0
elif MOTION == TIPTOE:
scale = 0.01
motion = yf.readBvhFile(motionName, scale)
if MOTION != WALK:
yme.removeJoint(motion, HEAD, False)
yme.removeJoint(motion, RIGHT_SHOULDER, False)
yme.removeJoint(motion, LEFT_SHOULDER, False)
if FOOT_PART_NUM == 1 and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
elif (FOOT_PART_NUM == 5 or FOOT_PART_NUM == 6) and MOTION != WALK:
yme.removeJoint(motion, RIGHT_TOES, False)
yme.removeJoint(motion, RIGHT_TOES_END, False)
yme.removeJoint(motion, LEFT_TOES, False)
yme.removeJoint(motion, LEFT_TOES_END, False)
if MOTION != WALK:
yme.removeJoint(motion, RIGHT_HAND_END, False)
yme.removeJoint(motion, LEFT_HAND_END, False)
yme.offsetJointLocal(motion, RIGHT_ARM, (.03, -.05, 0), False)
yme.offsetJointLocal(motion, LEFT_ARM, (-.03, -.05, 0), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(1, 0, 0), .01), False)
yme.rotateJointLocal(motion, HIP, mm.exp(mm.v3(0, 0, 1), -.01), False)
if FOOT_PART_NUM == 1:
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(2.5, -0.0, .3), -.5), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(2.5, 0.0, -.3), -.5), False)
if MOTION == WALK:
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1., -0.0, .0),
.4), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1., 0.0, -.0), .4), False)
#yme.rotateJointLocal(motion, SPINE1, mm.exp(mm.v3(1.,0.0,0.0), -.8), False)
if MOTION == FORWARD_JUMP:
yme.rotateJointLocal(motion, LEFT_UP_LEG,
mm.exp(mm.v3(0.0, .0, 1.), .08), False)
yme.rotateJointLocal(motion, LEFT_LEG, mm.exp(mm.v3(0.0, 1.0, 0.),
-.2), False)
if FOOT_PART_NUM == 6:
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_1,
(-0.045, -0.06, 0.03))
yme.addJoint(motion, LEFT_FOOT, LEFT_METATARSAL_3,
(0.045, -0.06, 0.03)) #-0.0037
yme.addJoint(motion, LEFT_METATARSAL_1, LEFT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_METATARSAL_3, LEFT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, LEFT_PHALANGE_1, 'LEFT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_PHALANGE_3, 'LEFT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, LEFT_FOOT, LEFT_CALCANEUS_1, (0.0, -0.06, -0.02))
yme.addJoint(motion, LEFT_CALCANEUS_1, 'LEFT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_1,
(0.045, -0.06, 0.03))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_METATARSAL_3,
(-0.045, -0.06 - 0.0062, 0.03 + 0.0035))
yme.addJoint(motion, RIGHT_METATARSAL_1, RIGHT_PHALANGE_1,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_METATARSAL_3, RIGHT_PHALANGE_3,
(0.0, 0.0, 0.0715))
yme.addJoint(motion, RIGHT_PHALANGE_1, 'RIGHT_PHALANGE_Dummy1',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_PHALANGE_3, 'RIGHT_PHALANGE_Dummy2',
(0.0, 0.0, 0.1))
yme.addJoint(motion, RIGHT_FOOT, RIGHT_CALCANEUS_1,
(0.0, -0.06, -0.02))
yme.addJoint(motion, RIGHT_CALCANEUS_1, 'RIGHT_CALCANEUS_Dummy1',
(0.0, 0.0, -0.1))
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(.0, 0.0, 1.0), 3.14), False)
'''
yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), 3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -3.14*0.3), False)
yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0, 0.0), -3.14*0.3), False)
'''
yme.rotateJointLocal(motion, LEFT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2012), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2172), False)
yme.rotateJointLocal(motion, LEFT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2172), False)
yme.rotateJointLocal(motion, RIGHT_FOOT,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2283), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(1.0, 0.0, 0.0), -0.2171), False)
yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1,
mm.exp(mm.v3(1.0, 0.0, 0.0), 0.2171), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3,
mm.exp(mm.v3(0.0, 0.0, 1.0), 0.0747), False)
'''
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -0.1), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_1, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, RIGHT_METATARSAL_3, mm.exp(mm.v3(0.0,0.0,1.0), 0.06), False)
yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -0.05), False)
'''
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0656), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0871), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0087), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), 0.0933), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(0.0,0.0,1.0), 0.0746), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0,0.0), -6.3773e-03), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, RIGHT_PHALANGE_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.25), False)
#yme.rotateJointLocal(motion, LEFT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_FOOT, mm.exp(mm.v3(1.0,0.0, 0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, RIGHT_CALCANEUS_1, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.16), False)
#yme.rotateJointLocal(motion, LEFT_METATARSAL_3, mm.exp(mm.v3(1.0,0.0,0.0), -math.pi*0.5), False)
yme.updateGlobalT(motion)
################
if MOTION == FORWARD_JUMP:
motion = motion[515:555]
elif MOTION == TAEKWONDO:
## Taekwondo base-step
motion = motion[0:31]
#motion = motion[564:600]
elif MOTION == TAEKWONDO2:
## Taekwondo base-step
#motion = motion[0:31+40]
## Taekwondo turning-kick
motion = motion[108:-1]
#motion = motion[108:109]
elif MOTION == KICK:
#motion = motion[141:-1]
#motion = motion[100:-1]
#motion = motion[58:-1]
motion = motion[82:-1]
#motion = motion[0:-1]
elif MOTION == STAND2:
motion = motion[1:-1]
elif MOTION == TIPTOE:
#motion = motion[183:440]
#motion = motion[350:410]
motion = motion[350:550]
motion[0:0] = [motion[0]] * 40
motion.extend([motion[-1]] * 5000)
# world, model
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 1000.
mcfg.defaultBoneRatio = .9
for name in massMap:
node = mcfg.addNode(name)
node.mass = massMap[name]
node = mcfg.getNode(HIP)
node.length = .2
node.width = .25
node = mcfg.getNode(SPINE1)
node.length = .2
node.offset = (0, 0, 0.1)
node = mcfg.getNode(SPINE)
node.width = .22
#node.length = .2 ####
if FOOT_PART_NUM == 1:
length1 = .35
width1 = .2
mass1 = 4.3
elif FOOT_PART_NUM == 6:
mass0 = .4
width0 = 0.028
length0 = 0.1
#Metatarsal1
length1 = .15
width1 = width0 * 3
mass1 = mass0 * 1.8
#Metatarsal3
length3 = .11
width3 = width0 * 3
mass3 = mass0 * 1.86
#Calcaneus1
length4 = .08
width4 = 0.15 * 1
mass4 = mass0 * 1.2 * 2.
#Phalange1
length5 = .08
width5 = width1
mass5 = mass0 * 1.355
#Phalange3
length7 = length5
width7 = width5
mass7 = mass5
#Talus
length8 = .13
width8 = width0 * 3
mass8 = mass0 * 2.
if FOOT_PART_NUM == 6:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(RIGHT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(RIGHT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(RIGHT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(RIGHT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(RIGHT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
node = mcfg.getNode(LEFT_FOOT)
node.length = length8
node.width = width8
node.mass = mass8
node = mcfg.getNode(LEFT_METATARSAL_1)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_METATARSAL_3)
node.length = length3
node.width = width3
node.mass = mass3
node = mcfg.getNode(LEFT_PHALANGE_1)
node.length = length5
node.width = width5
node.mass = mass5
node.offset = (0.0, 0.0, 0.03)
node = mcfg.getNode(LEFT_PHALANGE_3)
node.length = length7
node.width = width7
node.mass = mass7
node.offset = (0.0, 0.0, 0.01)
node = mcfg.getNode(LEFT_CALCANEUS_1)
node.length = length4
node.width = width4
node.mass = mass4
if FOOT_PART_NUM < 5:
node = mcfg.getNode(RIGHT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
node = mcfg.getNode(LEFT_FOOT)
node.length = length1
node.width = width1
node.mass = mass1
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (1 / 30.) / (stepsPerFrame)
#stepsPerFrame = 10
#wcfg.timeStep = (1/120.)/(stepsPerFrame)
#wcfg.timeStep = (1/1800.)
# parameter
config = {}
config['Kt'] = 200
config['Dt'] = 2 * (config['Kt']**.5) # tracking gain
config['Kl'] = .10
config['Dl'] = 2 * (config['Kl']**.5) # linear balance gain
config['Kh'] = 0.1
config['Dh'] = 2 * (config['Kh']**.5) # angular balance gain
config['Ks'] = 20000
config['Ds'] = 2 * (config['Ks']**.5) # penalty force spring gain
config['Bt'] = 1.
config['Bl'] = 1. #0.5
config['Bh'] = 1.
if FOOT_PART_NUM == 1:
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .3,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .5,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .3,
LEFT_UP_LEG: .1,
LEFT_LEG: .3
}
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: 1.,
SPINE1: .3,
RIGHT_FOOT: 1.,
LEFT_FOOT: 1.,
HIP: 1.,
RIGHT_UP_LEG: 1.,
RIGHT_LEG: 1.,
LEFT_UP_LEG: 1.,
LEFT_LEG: 1.
}
elif FOOT_PART_NUM == 6:
config['weightMap2'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .5,
SPINE1: .3,
RIGHT_FOOT: .7,
LEFT_FOOT: .7,
HIP: .5,
RIGHT_UP_LEG: .7,
RIGHT_LEG: .7,
LEFT_UP_LEG: .7,
LEFT_LEG: .7,
LEFT_METATARSAL_1: .7,
RIGHT_METATARSAL_1: .7,
LEFT_METATARSAL_3: .7,
RIGHT_METATARSAL_3: .7,
RIGHT_CALCANEUS_1: .7,
LEFT_CALCANEUS_1: .7,
LEFT_PHALANGE_1: .4,
LEFT_PHALANGE_3: .4,
RIGHT_PHALANGE_1: .4,
RIGHT_PHALANGE_3: .4
}
config['weightMap'] = {
RIGHT_ARM: .2,
RIGHT_FORE_ARM: .2,
LEFT_ARM: .2,
LEFT_FORE_ARM: .2,
SPINE: .3,
SPINE1: .2,
RIGHT_FOOT: .3,
LEFT_FOOT: .3,
HIP: .3,
RIGHT_UP_LEG: .1,
RIGHT_LEG: .2,
LEFT_UP_LEG: .1,
LEFT_LEG: .2,
LEFT_METATARSAL_1: .1,
RIGHT_METATARSAL_1: .1,
LEFT_METATARSAL_3: .1,
RIGHT_METATARSAL_3: .1,
RIGHT_CALCANEUS_1: .2,
LEFT_CALCANEUS_1: .2,
LEFT_PHALANGE_1: .1,
LEFT_PHALANGE_3: .1,
RIGHT_PHALANGE_1: .1,
RIGHT_PHALANGE_3: .1
}
'''
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.3, SPINE1:.3, RIGHT_FOOT:.3, LEFT_FOOT:.3, HIP:.5,
RIGHT_UP_LEG:.1, RIGHT_LEG:.3, LEFT_UP_LEG:.1, LEFT_LEG:.3,
LEFT_METATARSAL_3:.2, RIGHT_METATARSAL_3:.2, RIGHT_CALCANEUS:1.2, LEFT_CALCANEUS:1.2,
LEFT_PHALANGE_1:.2, LEFT_PHALANGE_3:.1, RIGHT_PHALANGE_1:.2, RIGHT_PHALANGE_3:.2}
config['weightMap2']={RIGHT_ARM:.2, RIGHT_FORE_ARM:.2, LEFT_ARM:.2, LEFT_FORE_ARM:.2, SPINE:.8, SPINE1:.3, RIGHT_FOOT:1., LEFT_FOOT:1., HIP:1.,
RIGHT_UP_LEG:1., RIGHT_LEG:1., LEFT_UP_LEG:1., LEFT_LEG:1.,
LEFT_METATARSAL_3:1., RIGHT_METATARSAL_3:1., RIGHT_CALCANEUS:.3, LEFT_CALCANEUS:.3,
LEFT_PHALANGE_1:.3, LEFT_PHALANGE_3:.3, RIGHT_PHALANGE_1:.3, RIGHT_PHALANGE_3:.3}
'''
config['supLink'] = LEFT_FOOT
config['supLink2'] = RIGHT_FOOT
#config['end'] = HIP
config['end'] = SPINE1
config['const'] = HIP
config['root'] = HIP
config['FootPartNum'] = FOOT_PART_NUM
if FOOT_PART_NUM == 6:
config['FootLPart'] = [
LEFT_FOOT, LEFT_METATARSAL_1, LEFT_METATARSAL_3, LEFT_PHALANGE_1,
LEFT_PHALANGE_3, LEFT_CALCANEUS_1
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_METATARSAL_1, RIGHT_METATARSAL_3,
RIGHT_PHALANGE_1, RIGHT_PHALANGE_3, RIGHT_CALCANEUS_1
]
else:
config['FootLPart'] = [
LEFT_FOOT, LEFT_TOES, LEFT_TARSUS, LEFT_TOES_SIDE_R,
LEFT_FOOT_SIDE_L, LEFT_FOOT_SIDE_R
]
config['FootRPart'] = [
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_TOES_SIDE_R,
RIGHT_FOOT_SIDE_L, RIGHT_FOOT_SIDE_R
]
return motion, mcfg, wcfg, stepsPerFrame, config
