def addJointSO3FromBvhJoint(self,
jointPosture,
bvhJoint,
channelValues,
scale=1.0):
localR = mm.I_SO3()
for channel in bvhJoint.channels:
if channel.channelType == 'XPOSITION':
jointPosture.rootPos[0] = channelValues[
channel.channelIndex] * scale
elif channel.channelType == 'YPOSITION':
jointPosture.rootPos[1] = channelValues[
channel.channelIndex] * scale
elif channel.channelType == 'ZPOSITION':
jointPosture.rootPos[2] = channelValues[
channel.channelIndex] * scale
elif channel.channelType == 'XROTATION':
localR = numpy.dot(
localR,
mm.rotX(mm.RAD * channelValues[channel.channelIndex]))
elif channel.channelType == 'YROTATION':
localR = numpy.dot(
localR,
mm.rotY(mm.RAD * channelValues[channel.channelIndex]))
elif channel.channelType == 'ZROTATION':
localR = numpy.dot(
localR,
mm.rotZ(mm.RAD * channelValues[channel.channelIndex]))
jointPosture.setLocalR(
jointPosture.skeleton.getElementIndex(bvhJoint.name), localR)
for i in range(len(bvhJoint.children)):
self.addJointSO3FromBvhJoint(jointPosture, bvhJoint.children[i],
channelValues)
def _calcJointLocalR(joint, extendedPoints, initRot, jointPosture):
if len(joint.children)!=1:
curRot = mmMath.I_SO3()
else:
p = extendedPoints[_getValidParentName(joint)] - extendedPoints[joint.name]
c = extendedPoints[joint.children[0].name] - extendedPoints[joint.name]
curRot = mmMath.getSO3FromVectors(p, c)
jointPosture.setLocalR(joint.name, numpy.dot(curRot, initRot[joint.name].transpose()))
for childJoint in joint.children:
_calcJointLocalR(childJoint, extendedPoints, initRot, jointPosture)
def test_zyx():
x, y, z = (1.,.5,2.)
print 'orig ', x, y, z
R = mm.I_SO3()
R = np.dot(R, mm.rotZ(z))
R = np.dot(R, mm.rotX(x))
R = np.dot(R, mm.rotY(y))
nz, nx, ny = cm.R2zxy_r(R)
print 'zxy_r', nx, ny, nz
R = mm.I_SO3()
R = np.dot(R, mm.rotY(y))
R = np.dot(R, mm.rotX(x))
R = np.dot(R, mm.rotZ(z))
nz, nx, ny = cm.R2zxy_s(R)
print 'zxy_s', nx, ny, nz
R = mm.I_SO3()
R = np.dot(R, mm.rotX(x))
R = np.dot(R, mm.rotY(y))
R = np.dot(R, mm.rotZ(z))
nx, ny, nz = cm.R2xyz_r(R)
print 'xyz_r', nx, ny, nz
def PDControl3(frame):
# Kp = 100.*70;
# Kd = 10.*3;
Kp = 1000.
Kd = 10.
desiredOri1 = mm.exp(axis1, deg1[0] * M_RADIAN)
desiredOriX = mm.exp(axisX, deg1[0] * M_RADIAN)
desiredOriY = mm.exp(axisY, deg1[0] * M_RADIAN)
desiredOriZ = mm.exp(axisZ, deg1[0] * M_RADIAN)
calcPDTorque(mm.I_SO3(), mm.exp(axisX, -90 * M_RADIAN),
base.getRotation(), body1.getRotation(),
base.getAngularVel(), body1.getAngularVel(), Kp, Kd,
M1)
def mm2Joint_Posture(pointPosture, pointTPose, jointSkeleton):
jointPosture = ym.JointPosture(jointSkeleton)
jointPosture.rootPos = pointPosture.pointMap[jointSkeleton.root.name]
extendedPoints = {}
for jointName, joint in jointSkeleton.joints.items():
if joint.parent == None:
extendedPoints[
'parent_' +
jointName] = pointTPose.pointMap[jointName] + numpy.array(
[1., 0., 0.])
extendedPoints[jointName] = pointTPose.pointMap[jointName]
elif len(joint.parent.children) > 1:
extendedPoints[jointName] = pointTPose.pointMap[joint.parent.name]
else:
extendedPoints[jointName] = pointTPose.pointMap[jointName]
initRot = {}
for jointName, joint in jointSkeleton.joints.items():
if len(joint.children) != 1:
curRot = mmMath.I_SO3()
else:
p = extendedPoints[_getValidParentName(joint)] - extendedPoints[
joint.name]
c = extendedPoints[joint.children[0].name] - extendedPoints[
joint.name]
curRot = mmMath.getSO3FromVectors(p, c)
initRot[joint.name] = curRot
extendedPoints2 = {}
for jointName, joint in jointSkeleton.joints.items():
if joint.parent == None:
extendedPoints2[
'parent_' +
jointName] = pointPosture.pointMap[jointName] + numpy.array(
[1., 0., 0.])
extendedPoints2[jointName] = pointPosture.pointMap[jointName]
elif len(joint.parent.children) > 1:
extendedPoints2[jointName] = pointPosture.pointMap[
joint.parent.name]
else:
extendedPoints2[jointName] = pointPosture.pointMap[jointName]
_calcJointLocalR(jointSkeleton.root, extendedPoints2, initRot,
jointPosture)
return jointPosture
RIGHT_FOOT, RIGHT_TOES, RIGHT_TARSUS, RIGHT_FOOT_SIDE_L,
RIGHT_FOOT_SIDE_R
]
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 buildMassMap():
node = mcfg.getNode('Spine')
node.width = .22
node = mcfg.getNode('RightFoot')
node.length = .25
node = mcfg.getNode('LeftFoot')
node.length = .25
return mcfg
g_motionDirConfigMap = {}
g_motionDirConfigMap['../../../Data/woody2/Motion/Physics2/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.4), '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/Picking/'] = \
{'footRot': mm.exp(mm.v3(1,0,0), -.5), 'yOffset': .0, 'scale':1.,'rootRot': mm.I_SO3()}
g_motionDirConfigMap['../../../Data/woody2/Motion/Samsung/'] = \
{'footRot': mm.rotX(-.5), 'yOffset': .0, 'scale':2.53999905501,'rootRot': mm.I_SO3()}
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)}
if __name__=='__main__':
#===============================================================================
# initialize motion
#===============================================================================
motionFiles = []
def simulateCallback(frame):
# seginfo
segIndex = seg_index[0]
curState = seginfo[segIndex]['state']
curInterval = yma.offsetInterval(acc_offset[0], seginfo[segIndex]['interval'])
stanceLegs = seginfo[segIndex]['stanceHips']
swingLegs = seginfo[segIndex]['swingHips']
stanceFoots = seginfo[segIndex]['stanceFoots']
swingFoots = seginfo[segIndex]['swingFoots']
swingKnees = seginfo[segIndex]['swingKnees']
groundHeight = seginfo[segIndex]['ground_height']
# maxStfPushFrame = seginfo[segIndex]['max_stf_push_frame']
prev_frame = frame-1 if frame>0 else 0
# prev_frame = frame
# information
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, upperMass, uppers)
## dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(frame), bodyMasses, totalMass)
## CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(frame), bodyMasses, totalMass)
# stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], frame)
# CMr_tar = CM_tar - stf_tar
dCM_tar = motion_seg.getJointVelocityGlobal(0, prev_frame)
CM_tar = motion_seg.getJointPositionGlobal(0, prev_frame)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, upperMass, uppers)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, upperMass, uppers)
# dCM_tar = yrp.getCM(motion_seg.getJointVelocitiesGlobal(prev_frame), bodyMasses, totalMass)
# CM_tar = yrp.getCM(motion_seg.getJointPositionsGlobal(prev_frame), bodyMasses, totalMass)
stf_tar = motion_seg.getJointPositionGlobal(stanceFoots[0], prev_frame)
CMr_tar = CM_tar - stf_tar
dCM = avg_dCM[0]
CM = controlModel.getJointPositionGlobal(0)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, upperMass, uppers)
# CM = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
CMreal = yrp.getCM(controlModel.getJointPositionsGlobal(), bodyMasses, totalMass)
stf = controlModel.getJointPositionGlobal(stanceFoots[0])
CMr = CM - stf
diff_dCM = mm.projectionOnPlane(dCM-dCM_tar, (1,0,0), (0,0,1))
diff_dCM_axis = np.cross((0,1,0), diff_dCM)
rd_vec1[0] = diff_dCM; rd_vecori1[0] = CM_tar
diff_CMr = mm.projectionOnPlane(CMr-CMr_tar, (1,0,0), (0,0,1))
# rd_vec1[0] = diff_CMr; rd_vecori1[0] = stf_tar
diff_CMr_axis = np.cross((0,1,0), diff_CMr)
direction = mm.normalize2(mm.projectionOnPlane(dCM_tar, (1,0,0), (0,0,1)))
# direction = mm.normalize2(mm.projectionOnPlane(dCM, (1,0,0), (0,0,1)))
directionAxis = np.cross((0,1,0), direction)
diff_dCM_sag, diff_dCM_cor = mm.projectionOnVector2(diff_dCM, direction)
# rd_vec1[0] = diff_dCM_sag; rd_vecori1[0] = CM_tar
diff_dCM_sag_axis = np.cross((0,1,0), diff_dCM_sag)
diff_dCM_cor_axis = np.cross((0,1,0), diff_dCM_cor)
diff_CMr_sag, diff_CMr_cor = mm.projectionOnVector2(diff_CMr, direction)
diff_CMr_sag_axis = np.cross((0,1,0), diff_CMr_sag)
diff_CMr_cor_axis = np.cross((0,1,0), diff_CMr_cor)
t = (frame-curInterval[0])/float(curInterval[1]-curInterval[0])
t_raw = t
if t>1.: t=1.
p_root = motion_stitch[frame].getJointPositionGlobal(0)
R_root = motion_stitch[frame].getJointOrientationGlobal(0)
motion_seg_orig.goToFrame(frame)
motion_seg.goToFrame(frame)
motion_stitch.goToFrame(frame)
motion_debug1.append(motion_stitch[frame].copy())
motion_debug1.goToFrame(frame)
motion_debug2.append(motion_stitch[frame].copy())
motion_debug2.goToFrame(frame)
motion_debug3.append(motion_stitch[frame].copy())
motion_debug3.goToFrame(frame)
# paper implementation
M_tc.append(motion_stitch[prev_frame])
M_tc.goToFrame(frame)
P_hat.append(M_tc[frame].copy())
P_hat.goToFrame(frame)
p_temp = ym.JointPosture(skeleton)
p_temp.rootPos = controlModel.getJointPositionGlobal(0)
p_temp.setJointOrientationsLocal(controlModel.getJointOrientationsLocal())
P.append(p_temp)
P.goToFrame(frame)
# stance foot stabilize
motion_stf_stabilize.append(motion_stitch[frame].copy())
motion_stf_stabilize.goToFrame(frame)
if STANCE_FOOT_STABILIZE:
for stanceFoot in stanceFoots:
R_target_foot = motion_seg[frame].getJointOrientationGlobal(stanceFoot)
R_current_foot = motion_stf_stabilize[frame].getJointOrientationGlobal(stanceFoot)
motion_stf_stabilize[frame].setJointOrientationGlobal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# R_target_foot = motion_seg[frame].getJointOrientationLocal(stanceFoot)
# R_current_foot = motion_stf_stabilize[frame].getJointOrientationLocal(stanceFoot)
# motion_stf_stabilize[frame].setJointOrientationLocal(stanceFoot, cm.slerp(R_current_foot, R_target_foot , stf_stabilize_func(t)))
# match stance leg
motion_match_stl.append(motion_stf_stabilize[frame].copy())
motion_match_stl.goToFrame(frame)
if MATCH_STANCE_LEG:
if curState!=yba.GaitState.STOP:
for i in range(len(stanceLegs)):
stanceLeg = stanceLegs[i]
stanceFoot = stanceFoots[i]
# # motion stance leg -> character stance leg as time goes
R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
R_character = controlModel.getJointOrientationGlobal(stanceLeg)
motion_match_stl[frame].setJointOrientationGlobal(stanceLeg, cm.slerp(R_motion, R_character, match_stl_func(t)))
# t_y = match_stl_func_y(t)
# t_xz = match_stl_func(t)
#
# R_motion = motion_match_stl[frame].getJointOrientationGlobal(stanceLeg)
# R_character = controlModel.getJointOrientationGlobal(stanceLeg)
# R = np.dot(R_character, R_motion.T)
# R_y, R_xz = mm.projectRotation((0,1,0), R)
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_xz, t_xz))
# motion_match_stl[frame].mulJointOrientationGlobal(stanceLeg, mm.scaleSO3(R_y, t_y))
# swing foot placement
motion_swf_placement.append(motion_match_stl[frame].copy())
motion_swf_placement.goToFrame(frame)
if SWING_FOOT_PLACEMENT:
t_swing_foot_placement = swf_placement_func(t);
if extended[0]:
R_swp_sag = prev_R_swp[0][0]
R_swp_cor = prev_R_swp[0][1]
else:
R_swp_sag = mm.I_SO3(); R_swp_cor = mm.I_SO3()
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_dCM_cor_axis * K_swp_vel_cor * -t_swing_foot_placement))
if np.dot(direction, diff_CMr_sag) < 0:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag * -t_swing_foot_placement))
else:
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_swp_vel_sag_faster * -t_swing_foot_placement))
R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_CMr_sag_axis * K_swp_pos_sag_faster * -t_swing_foot_placement))
R_swp_cor = np.dot(R_swp_cor, mm.exp(diff_CMr_cor_axis * K_swp_pos_cor * -t_swing_foot_placement))
for i in range(len(swingLegs)):
swingLeg = swingLegs[i]
swingFoot = swingFoots[i]
# save swing foot global orientation
# R_swf = motion_swf_placement[frame].getJointOrientationGlobal(swingFoot)
# rotate swing leg
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_sag)
motion_swf_placement[frame].mulJointOrientationGlobal(swingLeg, R_swp_cor)
# restore swing foot global orientation
# motion_swf_placement[frame].setJointOrientationGlobal(swingFoot, R_swf)
prev_R_swp[0] = (R_swp_sag, R_swp_cor)
# swing foot height
motion_swf_height.append(motion_swf_placement[frame].copy())
motion_swf_height.goToFrame(frame)
if SWING_FOOT_HEIGHT:
for swingFoot in swingFoots:
stanceFoot = stanceFoots[0]
# save foot global orientation
R_foot = motion_swf_height[frame].getJointOrientationGlobal(swingFoot)
R_stance_foot = motion_swf_height[frame].getJointOrientationGlobal(stanceFoot)
if OLD_SWING_HEIGHT:
height_tar = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
else:
height_tar = motion_swf_height[prev_frame].getJointPositionGlobal(swingFoot)[1] - groundHeight
d_height_tar = motion_swf_height.getJointVelocityGlobal(swingFoot, prev_frame)[1]
# height_tar += c_swf_mid_offset * swf_height_sine_func(t)
# motion_debug1[frame] = motion_swf_height[frame].copy()
# rotate
motion_swf_height[frame].rotateByTarget(controlModel.getJointOrientationGlobal(0))
# motion_debug2[frame] = motion_swf_height[frame].copy()
# motion_debug2[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
if OLD_SWING_HEIGHT:
height_cur = motion_swf_height[frame].getJointPositionGlobal(swingFoot)[1] - motion_swf_height[frame].getJointPositionGlobal(stanceFoot)[1]
else:
height_cur = controlModel.getJointPositionGlobal(swingFoot)[1] - halfFootHeight - c_swf_offset
d_height_cur = controlModel.getJointVelocityGlobal(swingFoot)[1]
if OLD_SWING_HEIGHT:
offset_height = (height_tar - height_cur) * swf_height_func(t) * c5
else:
offset_height = ((height_tar - height_cur) * c5
+ (d_height_tar - d_height_cur) * c6) * swf_height_func(t)
offset_sine = c_swf_mid_offset * swf_height_sine_func(t)
# offset_sine = 0.
offset = 0.
offset += offset_height
offset += offset_sine
if offset > 0.:
newPosition = motion_swf_height[frame].getJointPositionGlobal(swingFoot)
newPosition[1] += offset
aik.ik_analytic(motion_swf_height[frame], swingFoot, newPosition)
else:
if HIGHER_OFFSET:
newPosition = motion_swf_height[frame].getJointPositionGlobal(stanceFoot)
newPosition[1] -= offset
aik.ik_analytic(motion_swf_height[frame], stanceFoot, newPosition)
# return
# motion_debug3[frame] = motion_swf_height[frame].copy()
# motion_debug3[frame].translateByTarget(controlModel.getJointPositionGlobal(0))
motion_swf_height[frame].rotateByTarget(R_root)
# restore foot global orientation
motion_swf_height[frame].setJointOrientationGlobal(swingFoot, R_foot)
motion_swf_height[frame].setJointOrientationGlobal(stanceFoot, R_stance_foot)
if plot!=None:
plot.addDataPoint('debug1', frame, offset_height)
plot.addDataPoint('debug2', frame, height_tar - height_cur)
# plot.addDataPoint('diff', frame, diff)
# swing foot orientation
motion_swf_orientation.append(motion_swf_height[frame].copy())
motion_swf_orientation.goToFrame(frame)
if SWING_FOOT_ORIENTATION:
swf_orientation_func = yfg.concatenate([yfg.zero, yfg.hermite2nd, yfg.one], [.25, .75])
for swingFoot in swingFoots:
R_target_foot = motion_seg[curInterval[1]].getJointOrientationGlobal(swingFoot)
R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot, swf_orientation_func(t)))
# swf_stabilize_func = yfg.concatenate([yfg.hermite2nd, yfg.one], [c_taking_duration])
# push orientation
# for swingFoot in swingFoots:
# R_target_foot = motion_seg[frame].getJointOrientationGlobal(swingFoot)
# R_current_foot = motion_swf_orientation[frame].getJointOrientationGlobal(swingFoot)
# motion_swf_orientation[frame].setJointOrientationGlobal(swingFoot, cm.slerp(R_current_foot, R_target_foot , swf_stabilize_func(t)))
# stance foot push
motion_stf_push.append(motion_swf_orientation[frame].copy())
motion_stf_push.goToFrame(frame)
if STANCE_FOOT_PUSH:
for swingFoot in swingFoots:
# max_t = (maxStfPushFrame)/float(curInterval[1]-curInterval[0])
# stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [max_t*2])
stf_push_func = yfg.concatenate([yfg.sine, yfg.zero], [c_taking_duration*2])
R_swp_sag = mm.I_SO3()
# R_swp_sag = np.dot(R_swp_sag, mm.exp(diff_dCM_sag_axis * K_stp_vel * -stf_push_func(t)))
# if step_length_cur[0] < step_length_tar[0]:
# ratio = step_length_cur[0] / step_length_tar[0]
# R_max = maxmaxStfPushFrame
# R_zero =
R_swp_sag = np.dot(R_swp_sag, mm.exp((step_length_tar[0] - step_length_cur[0])*step_axis[0] * K_stp_pos * -stf_push_func(t)))
motion_stf_push[frame].mulJointOrientationGlobal(swingFoot, R_swp_sag)
# stance foot balancing
motion_stf_balancing.append(motion_stf_push[frame].copy())
motion_stf_balancing.goToFrame(frame)
if STANCE_FOOT_BALANCING:
R_stb = mm.exp(diff_dCM_axis * K_stb_vel * stf_balancing_func(t))
R_stb = np.dot(R_stb, mm.exp(diff_CMr_axis * K_stb_pos * stf_balancing_func(t)))
for stanceFoot in stanceFoots:
if frame < 5: continue
motion_stf_balancing[frame].mulJointOrientationGlobal(stanceFoot, R_stb)
# control trajectory
motion_control.append(motion_stf_balancing[frame].copy())
motion_control.goToFrame(frame)
#=======================================================================
# tracking with inverse dynamics
#=======================================================================
th_r = motion_control.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion_control.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion_control.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt)
#=======================================================================
# simulation
#=======================================================================
CP = mm.v3(0.,0.,0.)
F = mm.v3(0.,0.,0.)
avg_dCM[0] = mm.v3(0.,0.,0.)
# external force rendering info
del rd_forces[:]; del rd_force_points[:]
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
rd_forces.append(fi.force)
rd_force_points.append(controlModel.getBodyPositionGlobal(fi.targetBody) + -mm.normalize2(fi.force)*.2)
for i in range(stepsPerFrame):
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals, contactForces)
# apply external force
for fi in forceInfos:
if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
# # apply external force
# for fi in forceInfos:
# if fi.startFrame <= frame and frame < fi.startFrame + fi.duration*(1/frameTime):
# controlModel.applyBodyForceGlobal(fi.targetBody, fi.force)
vpWorld.step()
# yvu.align2D(controlModel)
if len(contactForces) > 0:
CP += yrp.getCP(contactPositions, contactForces)
F += sum(contactForces)
avg_dCM[0] += controlModel.getJointVelocityGlobal(0)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, upperMass, uppers)
# avg_dCM[0] += yrp.getCM(controlModel.getJointVelocitiesGlobal(), bodyMasses, totalMass)
# if len(stanceFoots)>0:
# avg_stf_v[0] += controlModel.getJointVelocityGlobal(stanceFoots[0])
# avg_stf_av[0] += controlModel.getJointAngVelocityGlobal(stanceFoots[0])
CP /= stepsPerFrame
F /= stepsPerFrame
avg_dCM[0] /= stepsPerFrame
# if len(stanceFoots)>0:
# avg_stf_v[0] /= stepsPerFrame
# avg_stf_av[0] /= stepsPerFrame
# rd_vec1[0] = avg_stf_av[0]; rd_vec1[0][0] = 0.; rd_vec1[0][2] = 0.
# rd_vecori1[0]= controlModel.getJointPositionGlobal(stanceFoots[0])
#=======================================================================
# segment editing
#=======================================================================
lastFrame = False
if SEGMENT_EDITING:
if curState==yba.GaitState.STOP:
if frame == len(motion_seg)-1:
lastFrame = True
elif (curState==yba.GaitState.LSWING or curState==yba.GaitState.RSWING) and t>c_min_contact_time:
swingID = lID if curState==yba.GaitState.LSWING else rID
contact = False
if swingID in bodyIDs:
minContactVel = 1000.
for i in range(len(bodyIDs)):
if bodyIDs[i]==swingID:
vel = controlModel.getBodyVelocityGlobal(swingID, contactPositionLocals[i])
vel[1] = 0
contactVel = mm.length(vel)
if contactVel < minContactVel: minContactVel = contactVel
if minContactVel < c_min_contact_vel: contact = True
extended[0] = False
if contact:
# print frame, 'foot touch'
lastFrame = True
acc_offset[0] += frame - curInterval[1]
elif frame == len(motion_seg)-1:
print frame, 'extend frame', frame+1
preserveJoints = []
# preserveJoints = [lFoot, rFoot]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee]
# preserveJoints = [lFoot, rFoot, lKnee, rKnee, lUpLeg, rUpLeg]
stanceKnees = [rKnee] if curState==yba.GaitState.LSWING else [lKnee]
preserveJoints = [stanceFoots[0], stanceKnees[0], stanceLegs[0]]
diff = 3
motion_seg_orig.extend([motion_seg_orig[-1]])
motion_seg.extend(ymt.extendByIntegration_root(motion_seg, 1, diff))
motion_stitch.extend(ymt.extendByIntegration_constant(motion_stitch, 1, preserveJoints, diff))
# # extend for swing foot ground speed matching & swing foot height lower
## extendedPostures = ymt.extendByIntegration(motion_stitch, 1, preserveJoints, diff)
## extendedPostures = [motion_stitch[-1]]
##
# extendFrameNum = frame - curInterval[1] + 1
# k = 1.-extendFrameNum/5.
# if k<0.: k=0.
# extendedPostures = ymt.extendByIntegrationAttenuation(motion_stitch, 1, preserveJoints, diff, k)
#
## if len(swingFoots)>0 and np.inner(dCM_tar, dCM)>0.:
## print frame, 'speed matching'
## R_swf = motion_stitch[-1].getJointOrientationGlobal(swingFoots[0])
##
## p_swf = motion_stitch[-1].getJointPositionGlobal(swingFoots[0])
## v_swf = motion_stitch.getJointVelocityGlobal(swingFoots[0], frame-diff, frame)
## a_swf = motion_stitch.getJointAccelerationGlobal(swingFoots[0], frame-diff, frame)
## p_swf += v_swf * (frameTime) + a_swf * (frameTime)*(frameTime)
## aik.ik_analytic(extendedPostures[0], swingFoots[0], p_swf)
##
## extendedPostures[0].setJointOrientationGlobal(swingFoots[0], R_swf)
#
# motion_stitch.extend(extendedPostures)
extended[0] = True
else:
if frame == len(motion_seg)-1: lastFrame = True
if lastFrame:
if segIndex < len(segments)-1:
print '%d (%d): end of %dth seg (%s, %s)'%(frame, frame-curInterval[1],segIndex, yba.GaitState.text[curState], curInterval)
if plot!=None: plot.addDataPoint('diff', frame, (frame-curInterval[1])*.01)
if len(stanceFoots)>0 and len(swingFoots)>0:
# step_cur = controlModel.getJointPositionGlobal(swingFoots[0]) - controlModel.getJointPositionGlobal(stanceFoots[0])
# step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(swingFoots[0]) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = controlModel.getJointPositionGlobal(0) - controlModel.getJointPositionGlobal(stanceFoots[0])
step_tar = motion_seg[curInterval[1]].getJointPositionGlobal(0) - motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
step_cur = mm.projectionOnPlane(step_cur, (1,0,0), (0,0,1))
step_tar = mm.projectionOnPlane(step_tar, (1,0,0), (0,0,1))
step_cur_sag, step_cur_cor = mm.projectionOnVector2(step_cur, direction)
step_tar_sag, step_tar_cor = mm.projectionOnVector2(step_tar, direction)
step_length_tar[0] = mm.length(step_tar_sag)
if np.inner(step_tar_sag, step_cur_sag) > 0:
step_length_cur[0] = mm.length(step_cur_sag)
else:
step_length_cur[0] = -mm.length(step_cur_sag)
step_axis[0] = directionAxis
# rd_vec1[0] = step_tar_sag
# rd_vecori1[0] = motion_seg[curInterval[1]].getJointPositionGlobal(stanceFoots[0])
# rd_vec2[0] = step_cur_sag
# rd_vecori2[0] = controlModel.getJointPositionGlobal(stanceFoots[0])
seg_index[0] += 1
curSeg = segments[seg_index[0]]
stl_y_limit_num[0] = 0
stl_xz_limit_num[0] = 0
del motion_seg_orig[frame+1:]
motion_seg_orig.extend(ymb.getAttachedNextMotion(curSeg, motion_seg_orig[-1], False, False))
del motion_seg[frame+1:]
del motion_stitch[frame+1:]
transitionLength = len(curSeg)-1
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, False))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, False))
d = motion_seg[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_seg.extend(ymb.getAttachedNextMotion(curSeg, d, True, False))
d = motion_control[-1] - curSeg[0]
d.rootPos[1] = 0.
motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, d, transitionLength, stitch_func, True, False))
# motion_seg.extend(ymb.getAttachedNextMotion(curSeg, motion_seg[-1], False, True))
# motion_stitch.extend(ymb.getStitchedNextMotion(curSeg, motion_control[-1], transitionLength, stitch_func, True, True))
else:
motion_seg_orig.append(motion_seg_orig[-1])
motion_seg.append(motion_seg[-1])
motion_stitch.append(motion_control[-1])
# rendering
motionModel.update(motion_ori[frame])
# motionModel.update(motion_seg[frame])
rd_CP[0] = CP
rd_CMP[0] = (CMreal[0] - (F[0]/F[1])*CMreal[1], 0, CMreal[2] - (F[2]/F[1])*CMreal[1])
if plot!=None:
plot.addDataPoint('zero', frame, 0)
plot.updatePoints()
def calcPDTorque(self, posture):
torques = {}
joint = posture.skeleton.root
R = mm.I_SO3()
self._calcPDTorqueJoint(joint, R, posture, torques)
return torques
def simulateCallback(frame):
curTime = time.time()
if frame % 30 == 1: pt[0] = time.time()
global g_initFlag
global forceShowFrame
global forceApplyFrame
global JsysPre
global JsupPreL
global JsupPreR
global JsupPre
global softConstPoint
global stage
global contactRendererName
global desCOMOffset
#motionModel.update(motion[0])
Kt, Kk, Kl, Kh, Ksc, Bt, Bl, Bh, B_CM, B_CMSd, B_Toe = viewer.GetParam(
)
Dt = 2 * (Kt**.5)
Dk = 2 * (Kk**.5)
Dl = 2 * (Kl**.5)
Dh = 2 * (Kh**.5)
Dsc = 2 * (Ksc**.5)
'''
if Bsc == 0.0 :
viewer.doc.showRenderer('softConstraint', False)
viewer.motionViewWnd.update(1, viewer.doc)
else:
viewer.doc.showRenderer('softConstraint', True)
renderer1 = viewer.doc.getRenderer('softConstraint')
renderer1.rc.setLineWidth(0.1+Bsc*3)
viewer.motionViewWnd.update(1, viewer.doc)
'''
pose = motion[0].copy()
def solveIK(desComPos,
desIdxs,
desPos,
desOri,
cmW=10.,
posW=1.,
oriW=1.):
numItr = 100
dt = .5
threshold = 0.1
for i in range(0, numItr):
jPart_IK = []
print '----iter num', i
IKModel.update(pose)
th_r_IK = pose.getDOFPositions()
jointPositions_IK = pose.getJointPositionsGlobal()
jointAxeses_IK = pose.getDOFAxeses()
linkPositions_IK = IKModel.getBodyPositionsGlobal()
linkInertias_IK = IKModel.getBodyInertiasGlobal()
CM_IK = yrp.getCM(linkPositions_IK, linkMasses, totalMass)
print CM_IK
P_IK = ymt.getPureInertiaMatrix(TO, linkMasses,
linkPositions_IK, CM_IK,
linkInertias_IK)
yjc.computeJacobian2(Jsys_IK, DOFs, jointPositions_IK,
jointAxeses_IK, linkPositions_IK,
allLinkJointMasks)
for j in range(0, len(desIdxs)):
jPart_IK.append(Jsys_IK[6 * desIdxs[j]:6 * desIdxs[j] + 6])
J_IK, JAngCom_IK = np.vsplit(np.dot(P_IK, Jsys_IK), 2)
dv_IK = cmW * (desComPos - CM_IK)
for j in range(0, len(desIdxs)):
J_IK = np.vstack((J_IK, jPart_IK[j]))
pos_IK = IKModel.getBodyPositionGlobal(desIdxs[j])
dv_IK = np.append(dv_IK, posW * (desPos[j] - pos_IK))
ori_IK = IKModel.getBodyOrientationGlobal(desIdxs[j])
dv_IK = np.append(dv_IK,
oriW * mm.logSO3(desOri[j] * ori_IK.T))
#print dv_IK[0:3]
dth_IK_solve = npl.lstsq(J_IK, dv_IK)
dth_IK_x = dth_IK_solve[0][:totalDOF]
ype.nested(dth_IK_x, dth_IK)
#print dth_IK[0][0:3]
th_IK = yct.getIntegralDOF(th_r_IK, dth_IK, dt)
pose.setDOFPositions(th_IK)
if np.dot(dv_IK, dv_IK) < threshold:
break
linkPositions_ref = motionModel.getBodyPositionsGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
#CM_IK_ref = footCenter + footCenterOffset
CM_IK_ref = CM_ref + footCenterOffset
#CM_IK_ref[1] = CM_ref[1] + footCenterOffset[1]
motion[0].skeleton.getJointIndex(config['supLink'])
#IKidxs = [indexFootL[0], indexFootR[0]]
#IKdesPos = [motionModel.getBodyPositionGlobal(indexFootL[0]), motionModel.getBodyPositionGlobal(indexFootR[0])]
#for i in range(0, 2):
# #IKdesPos[i] += ModelOffset
# IKdesPos[i][1] = 0.069
#IKori = [motionModel.getBodyOrientationGlobal(indexFootL[0]), motionModel.getBodyOrientationGlobal(indexFootR[0])]
#IKdesOri = [None]*2
#for i in range(0, 2):
# IKdesOri[i] = mm.I_SO3()
IKidxs = config['Phalange'][0:1] + config['Phalange'][3:4]
print IKidxs
IKdesPos = [None] * len(IKidxs)
IKdesOri = [None] * len(IKidxs)
for i in range(0, len(IKidxs)):
#print i
IKdesPos[i] = motionModel.getBodyPositionGlobal(IKidxs[i])
IKdesPos[i][1] = 0.03
IKdesOri[i] = mm.I_SO3()
print IKdesPos
solveIK(CM_IK_ref, IKidxs, IKdesPos, IKdesOri)
# tracking
th_r_ori = motion.getDOFPositions(frame)
th_r = copy.copy(th_r_ori)
global leftHipTimer
if viewer.objectInfoWnd.onLeftHip:
leftHipTimer = 60
viewer.objectInfoWnd.onLeftHip = False
if leftHipTimer > 0:
viewer.objectInfoWnd.comOffsetX.value(
0.14 * np.sin(2 * 3.14 * leftHipTimer / 60.))
#viewer.objectInfoWnd.comOffsetZ.value(0.04*np.cos(2*3.14*leftHipTimer/90.))
#B_Hipd = viewer.objectInfoWnd.labelLeftHip.value()
#newR1 = mm.exp(mm.v3(0.0,1.0,0.0), 3.14*0.5*B_Hipd/100.)
#idx = motion[0].skeleton.getJointIndex('LeftUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
#idx = motion[0].skeleton.getJointIndex('RightUpLeg')
#th_r[idx] = np.dot(th_r[idx], newR1)
leftHipTimer -= 1
timeReport[0] += time.time() - curTime
curTime = time.time()
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r,
Kt, Dt)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
ype.flatten(dth, dth_flat)
ype.flatten(ddth_c, ddth_c_flat)
# jacobian
refFootL = motionModel.getBodyPositionGlobal(supL)
refFootR = motionModel.getBodyPositionGlobal(supR)
positionFootL = [None] * footPartNum
positionFootR = [None] * footPartNum
for i in range(footPartNum):
positionFootL[i] = controlModel.getBodyPositionGlobal(
indexFootL[i])
positionFootR[i] = controlModel.getBodyPositionGlobal(
indexFootR[i])
linkPositions = controlModel.getBodyPositionsGlobal()
linkVelocities = controlModel.getBodyVelocitiesGlobal()
linkAngVelocities = controlModel.getBodyAngVelocitiesGlobal()
linkInertias = controlModel.getBodyInertiasGlobal()
jointPositions = controlModel.getJointPositionsGlobal()
jointAxeses = controlModel.getDOFAxeses()
CM = yrp.getCM(linkPositions, linkMasses, totalMass)
dCM = yrp.getCM(linkVelocities, linkMasses, totalMass)
CM_plane = copy.copy(CM)
CM_plane[1] = 0.
dCM_plane = copy.copy(dCM)
dCM_plane[1] = 0.
linkPositions_ref = motionModel.getBodyPositionsGlobal()
linkVelocities_ref = motionModel.getBodyVelocitiesGlobal()
linkAngVelocities_ref = motionModel.getBodyAngVelocitiesGlobal()
linkInertias_ref = motionModel.getBodyInertiasGlobal()
CM_ref = yrp.getCM(linkPositions_ref, linkMasses, totalMass)
CM_plane_ref = copy.copy(CM_ref)
CM_plane_ref[1] = 0.
P = ymt.getPureInertiaMatrix(TO, linkMasses, linkPositions, CM,
linkInertias)
dP = ymt.getPureInertiaMatrixDerivative(dTO, linkMasses,
linkVelocities, dCM,
linkAngVelocities,
linkInertias)
timeReport[1] += time.time() - curTime
curTime = time.time()
yjc.computeJacobian2(Jsys, DOFs, jointPositions, jointAxeses,
linkPositions, allLinkJointMasks)
timeReport[2] += time.time() - curTime
curTime = time.time()
# yjc.computeJacobianDerivative2(dJsys, DOFs, jointPositions, jointAxeses, linkAngVelocities, linkPositions, allLinkJointMasks)
if frame > 0:
dJsys = (Jsys - JsysPre) * 30.
else:
dJsys = (Jsys - Jsys)
JsysPre = Jsys.copy()
timeReport[3] += time.time() - curTime
curTime = time.time()
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
CP = yrp.getCP(contactPositions, contactForces)
for i in range(len(bodyIDsToCheck)):
controlModel.SetBodyColor(bodyIDsToCheck[i], 0, 0, 0, 255)
contactFlagFootL = [0] * footPartNum
contactFlagFootR = [0] * footPartNum
partialDOFIndex = [22, 22]
for i in range(len(bodyIDs)):
controlModel.SetBodyColor(bodyIDs[i], 255, 105, 105, 200)
index = controlModel.id2index(bodyIDs[i])
for j in range(len(indexFootL)):
if index == indexFootL[j]:
contactFlagFootL[j] = 1
for j in range(len(indexFootR)):
if index == indexFootR[j]:
contactFlagFootR[j] = 1
for j in range(0, footPartNum):
jAngFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
jAngFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
dJAngFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6][3:] #.copy()
dJAngFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6][3:] #.copy()
jFootR[j] = Jsys[6 * indexFootR[j]:6 * indexFootR[j] + 6] #.copy()
jFootL[j] = Jsys[6 * indexFootL[j]:6 * indexFootL[j] + 6] #.copy()
dJFootR[j] = dJsys[6 * indexFootR[j]:6 * indexFootR[j] +
6] #.copy()
dJFootL[j] = dJsys[6 * indexFootL[j]:6 * indexFootL[j] +
6] #.copy()
if footPartNum == 1:
desFCL = (controlModel.getBodyPositionGlobal(supL))
desFCR = (controlModel.getBodyPositionGlobal(supR))
else:
r = .5 + desCOMOffset
desFCL = (controlModel.getBodyPositionGlobal(indexFootL[0]) * r +
controlModel.getBodyPositionGlobal(indexFootL[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootL[1])
desFCR = (controlModel.getBodyPositionGlobal(indexFootR[0]) * r +
controlModel.getBodyPositionGlobal(indexFootR[1]) *
(1.0 - r)
) #controlModel.getBodyPositionGlobal(indexFootR[1])
desFC = desFCL + (desFCR - desFCL) / 2.0
desFC[1] = 0
rd_footCenter_des[0] = desFC.copy()
curRelCMVec = CM_plane - desFC
vecRatio = mm.length(curRelCMVec) * 0.
#print(frame, vecRatio)
footCenter = desFC - curRelCMVec * (vecRatio) #/10.0
footCenter = (
getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'LeftPhalange_1') +
getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') +
getBodyGlobalPos(controlModel, motion, 'RightPhalange_1')) / 4.
#footCenter = (getBodyGlobalPos(controlModel, motion, 'LeftCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'LeftTalus_1') + getBodyGlobalPos(controlModel, motion, 'RightCalcaneus_1') + getBodyGlobalPos(controlModel, motion, 'RightTalus_1'))/4.
footCenter_ref = refFootL + (refFootR - refFootL) / 2.0
#footCenter_ref[1] = 0.
footCenter[1] = 0.
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(), 0,
viewer.objectInfoWnd.comOffsetZ.value()
])
#footCenter += footCenterOffset
vecRatio = mm.length(curRelCMVec) * 0.
softConstPointOffset = -curRelCMVec * (vecRatio) #/10.0
#print(frame, vecRatio, softConstPointOffset)
desForeSupLAcc = [0, 0, 0]
desForeSupRAcc = [0, 0, 0]
totalNormalForce = [0, 0, 0]
for i in range(len(contactForces)):
totalNormalForce[0] += contactForces[i][0]
totalNormalForce[1] += contactForces[i][1]
totalNormalForce[2] += contactForces[i][2]
#print((totalMass*mm.s2v(wcfg.gravity))[1])
footCenterOffset = np.array([
viewer.objectInfoWnd.comOffsetX.value(),
viewer.objectInfoWnd.comOffsetY.value(),
viewer.objectInfoWnd.comOffsetZ.value()
])
######################
# optimization terms
######################
# linear momentum
CM_ref_plane = footCenter + footCenterOffset
dL_des_plane = Kl * totalMass * (CM_ref_plane -
CM_plane) - Dl * totalMass * dCM_plane
dL_des_plane[1] = Kl * totalMass * (CM_ref[1] + footCenterOffset[1] -
CM[1]) - Dl * totalMass * dCM[1]
# angular momentum
CP_ref = footCenter + footCenterOffset
timeStep = 30.
if CP_old[0] == None or CP == None:
dCP = None
else:
dCP = (CP - CP_old[0]) / (1 / timeStep)
CP_old[0] = CP
if CP != None and dCP != None:
ddCP_des = Kh * (CP_ref - CP) - Dh * (dCP)
CP_des = CP + dCP * (1 / timeStep) + .5 * ddCP_des * (
(1 / timeStep)**2)
#dH_des = np.cross((CP_des - CM), (dL_des_plane + totalMass*mm.s2v(wcfg.gravity)))
dH_des = np.cross(
(CP_des - CM_plane),
(dL_des_plane + totalMass * mm.s2v(wcfg.gravity)))
else:
dH_des = None
# momentum matrix
RS = np.dot(P, Jsys)
R, S = np.vsplit(RS, 2)
rs = np.dot((np.dot(dP, Jsys) + np.dot(P, dJsys)), dth_flat)
r_bias, s_bias = np.hsplit(rs, 2)
flagContact = True
if dH_des == None or np.any(np.isnan(dH_des)) == True:
flagContact = False
#viewer.doc.showRenderer('rd_grf_des', False)
#viewer.motionViewWnd.update(1, viewer.doc)
#else:
#viewer.doc.showRenderer('rd_grf_des', True)
#viewer.motionViewWnd.update(1, viewer.doc)
'''
0 : initial
1 : contact
2 : fly
3 : landing
'''
trackingW = w
#if checkAll(contactFlagFootR, 0) != 1 :
if 0: #stage == MOTION_TRACKING:
trackingW = w2
#stage = POWERFUL_BALANCING
Bt = Bt * 2
#######################
# optimization
#######################
mot.addTrackingTerms(problem, totalDOF, Bt, trackingW, ddth_des_flat)
#if flagContact == True:
# mot.addLinearTerms(problem, totalDOF, Bl, dL_des_plane, R, r_bias)
# mot.addAngularTerms(problem, totalDOF, Bh, dH_des, S, s_bias)
a_sup_2 = None
Jsup_2 = None
dJsup_2 = None
##############################
#if Jsup_2 != None:
# mot.addConstraint(problem, totalDOF, Jsup_2, dJsup_2, dth_flat, a_sup_2)
timeReport[4] += time.time() - curTime
curTime = time.time()
r = problem.solve()
problem.clear()
ype.nested(r['x'], ddth_sol)
rootPos[0] = controlModel.getBodyPositionGlobal(selectedBody)
localPos = [[0, 0, 0]]
timeReport[5] += time.time() - curTime
curTime = time.time()
for i in range(stepsPerFrame):
# apply penalty force
bodyIDs, contactPositions, contactPositionLocals, contactForces = vpWorld.calcPenaltyForce(
bodyIDsToCheck, mus, Ks, Ds)
vpWorld.applyPenaltyForce(bodyIDs, contactPositionLocals,
contactForces)
extraForce[0] = viewer.GetForce()
if (extraForce[0][0] != 0 or extraForce[0][1] != 0
or extraForce[0][2] != 0):
forceApplyFrame += 1
#vpWorld.applyPenaltyForce(selectedBodyId, localPos, extraForce)
controlModel.applyBodyForceGlobal(selectedBody, extraForce[0])
applyedExtraForce[0] = extraForce[0]
if forceApplyFrame * wcfg.timeStep > 0.1:
viewer.ResetForce()
forceApplyFrame = 0
controlModel.setDOFAccelerations(ddth_sol)
controlModel.solveHybridDynamics()
vpWorld.step()
if frame % 30 == 0:
print 'elapsed time for 30 frames:', time.time() - pt[0]
# rendering
rd_footCenter[0] = footCenter
rd_CM[0] = CM.copy()
rd_CM_plane[0] = CM_plane.copy()
rd_footCenter_ref[0] = footCenter_ref
rd_CM_plane_ref[0] = CM_ref.copy()
rd_CM_ref[0] = CM_ref.copy()
rd_CM_ref_vec[0] = (CM_ref - footCenter_ref) * 3.
rd_CM_vec[0] = (CM - CM_plane)
rd_CM_des[0] = CM_ref_plane.copy()
rd_CM_des[0][1] = .01
#rd_CM_plane[0][1] = 0.
if CP != None and dCP != None:
rd_CP[0] = CP
rd_CP_des[0] = CP_des
rd_dL_des_plane[0] = dL_des_plane
rd_dH_des[0] = dH_des
rd_grf_des[
0] = totalNormalForce # - totalMass*mm.s2v(wcfg.gravity)#dL_des_plane - totalMass*mm.s2v(wcfg.gravity)
rd_exf_des[0] = applyedExtraForce[0]
rd_root_des[0] = rootPos[0]
rd_CMP[0] = softConstPoint
rd_soft_const_vec[0] = controlModel.getBodyPositionGlobal(
constBody) - softConstPoint
#indexL = motion[0].skeleton.getJointIndex('Hips')
#indexR = motion[0].skeleton.getJointIndex('Spine1')
indexL = indexFootL[0]
indexR = indexFootR[0]
curAng = [controlModel.getBodyOrientationGlobal(indexL)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footL_vec[0] = np.copy(curAngY[0])
rd_footCenterL[0] = controlModel.getBodyPositionGlobal(indexL)
curAng = [controlModel.getBodyOrientationGlobal(indexR)]
curAngY = np.dot(curAng, np.array([0, 0, 1]))
rd_footR_vec[0] = np.copy(curAngY[0])
rd_footCenterR[0] = controlModel.getBodyPositionGlobal(indexR)
if (forceApplyFrame == 0):
applyedExtraForce[0] = [0, 0, 0]
timeReport[6] += time.time() - curTime
