def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global mcfg_motion
global viewer
np.set_printoptions(precision=4, linewidth=200)
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped_basic(
'../2_Test01_Dynamic.bvh')
mcfg_motion = mit.normal_mcfg()
viewer = hsv.hpSimpleViewer([100, 100, 1280, 960])
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion))
def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global mcfg_motion
global vpWorld
global controlModel
global totalDOF
global DOFs
global bodyIDsToCheck
global torques_nested
global ddth_des_flat
global dth_flat
global ddth_sol
global rd_cForces
global rd_cPositions
global rd_jointPos
global rd_cForcesControl
global rd_cPositionsControl
global rd_ForceControl
global rd_ForceDes
global rd_Position
global rd_PositionDes
global viewer
global motionModel
global solver
global IKModel
np.set_printoptions(precision=4, linewidth=200)
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_1()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_vchain_5()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_chiken_foot()
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('fastswim.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot_2('simpleJump_2.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_capsule('simpleJump_onebody.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump.bvh')
# motion, mcfg, wcfg, stepsPerFrame, config = mit.create_foot('simpleJump_long.bvh')
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_legs('legs_robust.bvh')
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
IKModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
solver = hik.numIkSolver(wcfg, motion[0], mcfg)
# vpWorld.SetIntegrator("RK4")
# vpWorld.SetIntegrator("IMPLICIT_EULER_FAST")
vpWorld.SetIntegrator("EULER")
vpWorld.SetGlobalDamping(0.9999)
# controlModel.initializeHybridDynamics()
controlModel.initializeForwardDynamics()
ModelOffset = np.array([0., 2.5, 0.])
controlModel.translateByOffset(ModelOffset)
motionModel.translateByOffset(ModelOffset)
vpWorld.initialize()
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
bodyIDsToCheck = range(vpWorld.getBodyNum())
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
torques_nested = ype.makeNestedList(DOFs)
rd_cForces = [None]
rd_cPositions = [None]
rd_cForcesControl = [None]
rd_cPositionsControl = [None]
rd_ForceControl = [None]
rd_ForceDes = [None]
rd_Position = [None]
rd_PositionDes = [None]
rd_jointPos = [None]
viewer = hsv.hpSimpleViewer(title='main_Test')
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motionModel', cvr.VpModelRenderer(
# motionModel, MOTION_COLOR, yr.POLYGON_FILL))
# viewer.doc.addRenderer('IKModel', cvr.VpModelRenderer(
# solver.model, MOTION_COLOR, yr.POLYGON_FILL))
viewer.doc.addRenderer('controlModel', cvr.VpModelRenderer(
controlModel, CHARACTER_COLOR, yr.POLYGON_FILL))
viewer.doc.addRenderer('rd_contactForcesControl', yr.VectorsRenderer(
rd_cForcesControl, rd_cPositionsControl, (255, 0, 0), .1))
viewer.doc.addRenderer('rd_contactForces', yr.VectorsRenderer(
rd_cForces, rd_cPositions, (0, 255, 0), .1))
viewer.doc.addRenderer('rd_contactForceControl', yr.VectorsRenderer(
rd_ForceControl, rd_Position, (0, 0, 255), .1))
viewer.doc.addRenderer('rd_contactForceDes', yr.VectorsRenderer(
rd_ForceDes, rd_PositionDes, (255, 0, 255), .1))
# viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos))
viewer.objectInfoWnd.add1DSlider(
'PD gain', minVal=0., maxVal=500., initVal=10., valStep=.1)
viewer.objectInfoWnd.add1DSlider(
'Joint Damping', minVal=1., maxVal=2000., initVal=35., valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'steps per frame', minVal=1., maxVal=200., initVal=config['stepsPerFrame'], valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'1/simul speed', minVal=1., maxVal=100., initVal=config['simulSpeedInv'], valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'normal des force min', minVal=0., maxVal=1000., initVal=80., valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'normal des force max', minVal=0., maxVal=1000., initVal=80., valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'des force begin', minVal=0., maxVal=len(motion) - 1, initVal=70., valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'des force dur', minVal=1., maxVal=len(motion) - 1, initVal=5., valStep=1.)
viewer.objectInfoWnd.add1DSlider(
'force weight', minVal=-10., maxVal=10., initVal=0., valStep=.01)
viewer.objectInfoWnd.add1DSlider(
'LCP weight', minVal=-10., maxVal=10., initVal=0., valStep=.01)
viewer.objectInfoWnd.add1DSlider(
'tau weight', minVal=-10., maxVal=10., initVal=0., valStep=.01)
viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump)
viewer.objectInfoWnd.addBtn('image seq dump', viewer.motionViewWnd.dumpMov)
viewer.cForceWnd.addDataSet('expForce', FL_BLACK)
viewer.cForceWnd.addDataSet('desForceMin', FL_RED)
viewer.cForceWnd.addDataSet('desForceMax', FL_RED)
viewer.cForceWnd.addDataSet('realForce', FL_GREEN)
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
print("(index, id, name)")
for i in range(controlModel.getBodyNum()):
print(i, controlModel.index2id(i), controlModel.index2name(i))
def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global mcfg_motion
global viewer
global rd_jointPos
global rd_tibia
global rd_forefoot
global rd_backfoot
global rd_midfoot
global infoNames
global infoData
global markerName
global markerPosi
global tibia_marker_idx
global backfoot_marker_idx
global forefoot_marker_idx
global midfoot_marker_idx
global rd_tibia_cog
global rd_backfoot_cog
global rd_midfoot_cog
global rd_tibia_tri
global rd_backfoot_tri
global rd_midfoot_tri
global rd_forefoot_tri
np.set_printoptions(precision=4, linewidth=200)
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_biped_basic(
'../2_Test01_Dynamic.bvh')
mcfg_motion = mit.normal_mcfg()
infoNames, infoData, markerName, markerPosi = trc.import_trc(
'../trc/2_test01_dynamic.trc')
# infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/2_test02_dynamic.trc')
# infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/P_1_test01_dynamic.trc')
# infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/P_2_test01_dynamic.trc')
# infoNames, infoData, markerName, markerPosi = trc.import_trc('../trc/P_3_test01_dynamic.trc')
info = dict(zip(infoNames, infoData))
numFrame = int(info['NumFrames']) - 1
motion[:] = [motion[0]] * numFrame
rd_jointPos = [None]
rd_tibia = [None]
rd_backfoot = [None]
rd_midfoot = [None]
rd_forefoot = [None]
rd_tibia_tri = [None]
rd_backfoot_tri = [None]
rd_midfoot_tri = [None]
rd_forefoot_tri = [None]
rd_tibia_cog = [None]
rd_backfoot_cog = [None]
rd_midfoot_cog = [None]
viewer = hsv.hpSimpleViewer([100, 100, 1280, 960])
viewer.doc.addObject('motion', motion)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion))
viewer.doc.addRenderer(
'rd_jointPos',
yr.PointsRenderer(rd_jointPos, color=(255, 0, 0), pointStyle=1))
viewer.doc.addRenderer(
'rd_tibia',
yr.PointsRenderer(rd_tibia, color=(255, 0, 255), pointStyle=1))
viewer.doc.addRenderer('rd_tibia_tri',
yr.LinesRenderer(rd_tibia_tri, color=(255, 0, 255)))
viewer.doc.addRenderer(
'rd_backfoot',
yr.PointsRenderer(rd_backfoot, color=(0, 0, 255), pointStyle=1))
viewer.doc.addRenderer(
'rd_backfoot_tri', yr.LinesRenderer(rd_backfoot_tri,
color=(0, 0, 255)))
viewer.doc.addRenderer(
'rd_midfoot',
yr.PointsRenderer(rd_midfoot, color=(0, 255, 0), pointStyle=1))
viewer.doc.addRenderer('rd_midfoot_tri',
yr.LinesRenderer(rd_midfoot_tri, color=(0, 255, 0)))
viewer.doc.addRenderer(
'rd_forefoot',
yr.PointsRenderer(rd_forefoot, color=(0, 0, 0), pointStyle=1))
viewer.doc.addRenderer('rd_forefoot_tri',
yr.LinesRenderer(rd_forefoot_tri, color=(0, 0, 0)))
# viewer.doc.addRenderer('rd_tibia_cog', yr.PointsRenderer(rd_tibia_cog, color=(255,0,255), pointStyle=0))
# viewer.doc.addRenderer('rd_backfoot_cog', yr.PointsRenderer(rd_backfoot_cog, color=(0,0,255), pointStyle=0))
# viewer.doc.addRenderer('rd_midfoot_cog', yr.PointsRenderer(rd_midfoot_cog, color=(0,255,0), pointStyle=0))
# viewer.doc.addRenderer('rd_tibia_cog', yr.LinesRenderer(rd_tibia_cog, color=(255,0,255)))
# viewer.doc.addRenderer('rd_backfoot_cog', yr.LinesRenderer(rd_backfoot_cog, color=(0,0,255)))
# viewer.doc.addRenderer('rd_midfoot_cog', yr.LinesRenderer(rd_midfoot_cog, color=(0,255,0)))
viewer.objectInfoWnd.add1DSlider('PD gain',
minVal=0.,
maxVal=1000.,
initVal=180.,
valStep=.1)
viewer.objectInfoWnd.addBtn('image', viewer.motionViewWnd.dump)
viewer.objectInfoWnd.addBtn('movie', viewer.motionViewWnd.dumpMov)
# viewer.cForceWnd.addDataSet('midfoot_height', FL_BLACK)
# viewer.cForceWnd.addDataSet('midfoot_top_angle', FL_GREEN)
# viewer.cForceWnd.addDataSet('midfoot_front_angle', FL_RED)
print info
print markerName
'''
['Top.Head', 'Front.Head', 'Rear.Head',
'R.Shoulder', 'R.Offset', 'R.Elbow', 'R.Wrist',
'L.Shoulder', 'L.Elbow', 'L.Wrist', 'R.ASIS', 'L.ASIS',
'V.Sacral',
'R.Thigh', 'R.Knee', 'R.Shank', 'R.Ankle', 'R.Heel', 'R.Toe',
'L.Thigh', 'L.Knee', 'L.Shank', 'L.Ankle', 'L.Heel', 'L.Toe',
'R1', 'R2', 'R3', 'R4', 'R5', 'R6', 'R7',
'L1', 'L2', 'L3', 'L4', 'L5', 'L6', 'L7',
'V_Mid_ASIS', 'V_Pelvis_Origin', 'V_R.Hip_JC', 'V_L.Hip_JC', 'V_R.Knee_JC', 'V_L.Knee_JC', 'V_R.Ankle_JC', 'V_L.Ankle_JC', 'V_Mid_Hip', 'V_Mid_Shoulder', 'V_R.Hand', 'V_L.Hand', 'V_R.Toe_Offset_Static', 'V_L.Toe_Offset_Static', 'V_R.Toe_Offset', 'V_L.Toe_Offset']
'''
'''
['LFWT', 'RFWT', 'VBWT',
'LKNE', 'LANK', 'LMT5', 'LMT1', 'LHEL', 'LTOE',
'RKNE', 'RANK', 'RMT5', 'RMT1', 'RHEL', 'RTOE',
'T10', 'FHEAD', 'THEAD', 'RHEAD',
'LBSH', 'LELB', 'LOWR', 'RBSH', 'RELB', 'ROWR',
'LTHI', 'RTHI', 'LSHAN', 'RSHAN',
'LIHEL', 'LOHEL', 'RIHEL', 'ROHEL', 'LMT3', 'LNAVI', 'LCUNE', 'RMT3', 'RNAVI', 'RCUNE']
'''
tibia_marker = ['RKNE', 'RSHAN', 'RANK', 'LKNE', 'LSHAN', 'LANK']
forefoot_marker = [
'RTOE', 'RMT1', 'RMT3', 'RMT5', 'LTOE', 'LMT1', 'LMT3', 'LMT5'
]
midfoot_marker = ['RCUNE', 'RNAVI', 'LCUNE', 'LNAVI']
backfoot_marker = ['RHEL', 'RIHEL', 'ROHEL', 'LHEL', 'LIHEL', 'LOHEL']
tibia_marker_idx = [markerName.index(i) for i in tibia_marker]
midfoot_marker_idx = [markerName.index(i) for i in midfoot_marker]
forefoot_marker_idx = [markerName.index(i) for i in forefoot_marker]
backfoot_marker_idx = [markerName.index(i) for i in backfoot_marker]
print tibia_marker_idx
print forefoot_marker_idx
print midfoot_marker_idx
print backfoot_marker_idx
midfoot_height = []
midfoot_top_angle = []
midfoot_front_angle = []
midfoot_length = []
for frame in range(numFrame):
tibia_points = [
np.array(markerPosi[frame][i]) for i in tibia_marker_idx[:3]
]
backfoot_points = [
np.array(markerPosi[frame][i]) for i in backfoot_marker_idx[:3]
]
midfoot_points = [
np.array(markerPosi[frame][i]) for i in midfoot_marker_idx[:2]
]
forefoot_points = [
np.array(markerPosi[frame][i]) for i in forefoot_marker_idx[:4]
]
center, basis = getCenterAndBasisFromPoints(backfoot_points)
basis = [basis[2], basis[0], basis[1]]
rotation = np.zeros((3, 3))
for i in range(3):
rotation[i, :] = basis[i]
offset = np.array((0., 0., 0.))
tibia_trans_points = [
np.dot(rotation, point - center) + offset for point in tibia_points
]
backfoot_trans_points = [
np.dot(rotation, point - center) + offset
for point in backfoot_points
]
midfoot_trans_points = [
np.dot(rotation, point - center) + offset
for point in midfoot_points
]
forefoot_trans_points = [
np.dot(rotation, point - center) + offset
for point in forefoot_points
]
# viewer.cForceWnd.insertData('midfoot_height', frame, sum(midfoot_trans_points)[1]*1000/2.)
midfoot_height.append(sum(midfoot_trans_points)[1] * 1000. / 2.)
mf_ang_vec = midfoot_trans_points[1] - midfoot_trans_points[0]
mf_front_ang = math.asin(
abs(mf_ang_vec[1]) / npl.norm(mf_ang_vec)) * 180. / math.pi
mf_top_ang = math.atan2(mf_ang_vec[2], -mf_ang_vec[0]) * 180. / math.pi
midfoot_top_angle.append(mf_top_ang)
midfoot_front_angle.append(mf_front_ang)
midfoot_length.append(
npl.norm(midfoot_trans_points[1] - midfoot_trans_points[0]))
# viewer.cForceWnd.insertData('midfoot_top_angle', frame, mf_top_ang*50)
# viewer.cForceWnd.insertData('midfoot_front_angle', frame, mf_front_ang)
plt.figure(1)
plt.subplot(311)
# plt.ylim(-100, 100)
plt.ylabel('height(mm)')
plt.plot(midfoot_length)
# plt.plot(midfoot_height)
plt.subplot(312)
plt.ylim(0, 30)
plt.ylabel('front angle(deg)')
plt.plot(midfoot_front_angle)
plt.subplot(313)
plt.ylim(40, 65)
plt.ylabel('top angle(deg)')
plt.plot(midfoot_top_angle)
plt.savefig('hehe.pdf')
def init():
global motion
global mcfg
global wcfg
global stepsPerFrame
global config
global mcfg_motion
global vpWorld
global controlModel
global totalDOF
global DOFs
global bodyIDsToCheck
global torques_nested
global ddth_des_flat
global dth_flat
global ddth_sol
global rd_cForces
global rd_cPositions
global rd_jointPos
global rd_cForcesControl
global rd_cPositionsControl
global rd_ForceControl
global rd_ForceDes
global rd_Position
global rd_PositionDes
global viewer
np.set_printoptions(precision=4, linewidth=200)
motion, mcfg, wcfg, stepsPerFrame, config = mit.create_legs()
mcfg_motion = mit.normal_mcfg()
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
# vpWorld.SetIntegrator("RK4")
# vpWorld.SetIntegrator("IMPLICIT_EULER_FAST")
vpWorld.SetIntegrator("EULER")
# vpWorld.SetGlobalDamping(0.001)
# controlModel.initializeHybridDynamics()
controlModel.initializeForwardDynamics()
ModelOffset = np.array([0., 5.5, 0.])
controlModel.translateByOffset(ModelOffset)
totalDOF = controlModel.getTotalDOF()
DOFs = controlModel.getDOFs()
bodyIDsToCheck = range(vpWorld.getBodyNum())
# flat data structure
ddth_des_flat = ype.makeFlatList(totalDOF)
dth_flat = ype.makeFlatList(totalDOF)
ddth_sol = ype.makeNestedList(DOFs)
torques_nested = ype.makeNestedList(DOFs)
rd_cForces = [None]
rd_cPositions = [None]
rd_cForcesControl = [None]
rd_cPositionsControl = [None]
rd_ForceControl = [None]
rd_ForceDes = [None]
rd_Position = [None]
rd_PositionDes = [None]
rd_jointPos = [None]
viewer = hsv.hpSimpleViewer()
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer(
'controlModel',
cvr.VpModelRenderer(controlModel, CHARACTER_COLOR, yr.POLYGON_FILL))
viewer.doc.addRenderer(
'rd_contactForcesControl',
yr.VectorsRenderer(rd_cForcesControl, rd_cPositionsControl,
(255, 0, 0), .1))
viewer.doc.addRenderer(
'rd_contactForces',
yr.VectorsRenderer(rd_cForces, rd_cPositions, (0, 255, 0), .1))
viewer.doc.addRenderer(
'rd_contactForceControl',
yr.VectorsRenderer(rd_ForceControl, rd_Position, (0, 0, 255), .1))
viewer.doc.addRenderer(
'rd_contactForceDes',
yr.VectorsRenderer(rd_ForceDes, rd_PositionDes, (255, 0, 255), .1))
# viewer.doc.addRenderer('rd_jointPos', yr.PointsRenderer(rd_jointPos))
viewer.objectInfoWnd.add1DSlider('PD gain',
minVal=0.,
maxVal=1000.,
initVal=180.,
valStep=.1)
viewer.objectInfoWnd.add1DSlider('Joint Damping',
minVal=1.,
maxVal=2000.,
initVal=35.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('steps per frame',
minVal=1.,
maxVal=200.,
initVal=config['stepsPerFrame'],
valStep=1.)
viewer.objectInfoWnd.add1DSlider('1/simul speed',
minVal=1.,
maxVal=100.,
initVal=config['simulSpeedInv'],
valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force min',
minVal=0.,
maxVal=3000.,
initVal=80.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('normal des force max',
minVal=0.,
maxVal=3000.,
initVal=80.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force begin',
minVal=0.,
maxVal=len(motion) - 1,
initVal=70.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('des force dur',
minVal=0.,
maxVal=len(motion) - 1,
initVal=20.,
valStep=1.)
viewer.objectInfoWnd.add1DSlider('force weight',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('tracking weight',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.objectInfoWnd.add1DSlider('tau weight',
minVal=-10.,
maxVal=10.,
initVal=0.,
valStep=.01)
viewer.cForceWnd.addDataSet('expForce', FL_BLACK)
viewer.cForceWnd.addDataSet('desForceMin', FL_RED)
viewer.cForceWnd.addDataSet('desForceMax', FL_RED)
viewer.cForceWnd.addDataSet('realForce', FL_GREEN)
for i in range(motion[0].skeleton.getJointNum()):
print(i, motion[0].skeleton.getJointName(i))
print "(index, id, name)"
for i in range(controlModel.getBodyNum()):
print(i, controlModel.index2id(i), controlModel.index2name(i))