def simulateCallback(self, frame):
global ddth_des_flat
global stepsPerFrame
global wcfg
global vpWorld
# reload(tf)
motionModel.update(motion[0])
self.frame = frame
print("main:frame : ", frame)
# motionModel.update(motion[0])
self.timeIndex = 0
self.setTimeStamp()
# IK solver
'''
solver.clear()
solver.setInitPose(motion[0])
cVpBodyIds, cPositions, cPositionsLocal, cVelocities = vpWorld.getContactPoints(bodyIDsToCheck)
if len(cVpBodyIds) > 1:
solver.addConstraints(cVpBodyIds[1], cPositionsLocal[1], np.array((0., 0., 0.)), None, (False, True, False, False))
solver.addConstraints(cVpBodyIds[3], cPositionsLocal[3], np.array((0., 0., 0.)), None, (False, True, False, False))
solver.addConstraints(cVpBodyIds[5], cPositionsLocal[5], np.array((0., 0., 0.)), None, (False, True, False, False))
# solver.solve(controlModel, np.array((0., .15 + .05*math.sin(frame/10.), 0.)))
'''
# constant setting
# (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals()
getVal = viewer.objectInfoWnd.getVal
Kt = getVal('PD gain')
damp = getVal('Joint Damping')
stepsPerFrame = getVal('steps per frame')
simulSpeedInv = getVal('1/simul speed')
wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame)
vpWorld.SetTimeStep(wcfg.timeStep)
Dt = 2. * (Kt**.5)/20.
# Dt = 0.
controlModel.SetJointsDamping(damp)
# controlModel.SetJointsDamping(1.)
wLCP = math.pow(2., getVal('LCP weight'))
wForce = math.pow(2., getVal('force weight'))
wTorque = math.pow(2., getVal('tau weight'))
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
weightMapTuple = config['weightMapTuple']
weightMapTuple = None
ddth_des = yct.getDesiredDOFAccelerations(th_r, th, dth_r, dth, ddth_r, Kt, Dt, weightMapTuple)
ddth_c = controlModel.getDOFAccelerations()
ype.flatten(ddth_des, ddth_des_flat)
desForceFrameBegin = getVal('des force begin')
desForceDuration = getVal('des force dur') * simulSpeedInv
desForceFrame = [
desForceFrameBegin, desForceFrameBegin + desForceDuration]
desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration
desNormalForceMin = getVal('normal des force min')
desNormalForceMax = getVal('normal des force max')
desNormalForce = desNormalForceMin
if desForceFrame[0] <= frame <= desForceFrame[1]:
desNormalForce = desNormalForceMin * \
(1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame
totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.])
# totalForce = np.array([-desNormalForce, 34.3, 0., 0., 0., 0.])
# totalForce = np.array([0., 34.3, desNormalForce, 0., 0., 0.])
# totalForce = np.array([50., 150.])
torques = None
ddth_des_flat[0:6] = [0.] * 6
self.setTimeStamp()
simulContactForces = np.zeros(3)
cBodyIDs = None
cPositions = None
cPositionLocals = None
cForces = None
cBodyIDsControl = None
cPositionsControl = None
cPositionLocalsControl = None
cForcesControl = None
# if desForceFrame[0] <= frame <= desForceFrame[1]:
if True:
# totalForceImpulse = stepsPerFrame * totalForce
cBodyIDsControl, cPositionsControl, cPositionLocalsControl, cForcesControl, torques \
= hls.calcLCPbasicControl(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, [wLCP, wTorque, wForce], ddth_des_flat)
# if cForces is not None:
# print "control: ", sum(cForces)
sumControlForce = np.array([0.]*6)
if cForcesControl is not None:
sumControlForce = np.hstack((sum(cForcesControl), np.array([0., 0., 0.])))
timeStamp = None
torque_None = False
if not (desForceFrame[0] <= frame <= desForceFrame[1]) or (torques is None):
torque_None = True
torques = ddth_des_flat
elif np.linalg.norm(sumControlForce - totalForce) > np.linalg.norm(totalForce):
print("control failed!")
torque_None = True
torques = ddth_des_flat
else:
torques *= 1.
for i in range(int(stepsPerFrame)):
if i % 5 == 0:
cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
if i % 5 == 0 and len(cBodyIDs) > 0:
# apply contact forces
if False and not torque_None:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForcesControl)
simulContactForces += sum(cForcesControl)
else:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals, cForces)
simulContactForces += sum(cForces)
# simulContactForces += sum(cForces)
ype.nested(torques, torques_nested)
controlModel.setDOFTorques(torques_nested[1:])
vpWorld.step()
self.setTimeStamp()
# rendering expected force
del rd_cForcesControl[:]
del rd_cPositionsControl[:]
if cBodyIDsControl is not None:
# print cBodyIDsControl
for i in range(len(cBodyIDsControl)):
# print expected force
rd_cForcesControl.append(cForcesControl[i].copy() /50.)
rd_cPositionsControl.append(cPositionsControl[i].copy())
# rendering sum of expected force
del rd_ForceControl[:]
del rd_Position[:]
if cForcesControl is not None:
# print expected force
rd_ForceControl.append(sum(cForcesControl) /50.)
rd_Position.append(np.array([0., 0., 0.1]))
# graph expected force
if cForcesControl is not None:
sumForce = sum(cForcesControl)
if sumForce[1] > 10000:
sumForce[1] = 10000
viewer.cForceWnd.insertData('expForce', frame, sumForce[1])
else:
viewer.cForceWnd.insertData('expForce', frame, 0.)
# rendering calculated forces
del rd_cForces[:]
del rd_cPositions[:]
for i in range(len(cBodyIDs)):
# print calculated force
rd_cForces.append(cForces[i].copy() / 50.)
rd_cPositions.append(cPositions[i].copy())
# rendering joint position
del rd_jointPos[:]
for i in range(motion[0].skeleton.getJointNum()):
rd_jointPos.append(motion[frame].getJointPositionGlobal(i))
# rendering desired force
del rd_ForceDes[:]
del rd_PositionDes[:]
# rd_ForceDes.append(totalForce/50.)
rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.)
rd_PositionDes.append(np.array([0., 0., 0.]))
# if self.cForces is not None:
# rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.])
# rd_PositionDes.append(np.array([0., 0., -0.1]))
# graph calculated force
if cForces is not None:
sumForce = sum(cForces)
# viewer.cForceWnd.insertData('realForce', frame, sumForce[1])
viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame)
else:
viewer.cForceWnd.insertData('realForce', frame, 0.)
# viewer.cForceWnd.insertData('realForce', frame, simulContactForces[1]/stepsPerFrame)
# graph desired force
if desForceFrame[0] <= frame <= desForceFrame[1]:
viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1])
# viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * 1.0)
# viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1)
else:
viewer.cForceWnd.insertData('desForceMin', frame, 0.)
viewer.cForceWnd.insertData('desForceMax', frame, 0.)
self.setTimeStamp()
def simulateCallback(self, frame):
global ddth_des_flat
global stepsPerFrame
global wcfg
global vpWorld
# reload(tf)
self.frame = frame
print "main:frame : ", frame
# motionModel.update(motion[0])
self.timeIndex = 0
self.setTimeStamp()
# constant setting
# (Kt, damp, stepsPerFrame, simulSpeedInv) = viewer.objectInfoWnd.getVals()
getVal = viewer.objectInfoWnd.getVal
Kt = getVal('PD gain')
damp = getVal('Joint Damping')
stepsPerFrame = getVal('steps per frame')
simulSpeedInv = getVal('1/simul speed')
wcfg.timeStep = 1 / (30. * simulSpeedInv * stepsPerFrame)
vpWorld.SetTimeStep(wcfg.timeStep)
# Dt = 2. * (Kt**.5)
Dt = 0.
controlModel.SetJointsDamping(damp)
wForce = math.pow(2., getVal('force weight'))
wTorque = math.pow(2., getVal('tau weight'))
# tracking
th_r = motion.getDOFPositions(frame)
th = controlModel.getDOFPositions()
dth_r = motion.getDOFVelocities(frame)
dth = controlModel.getDOFVelocities()
ddth_r = motion.getDOFAccelerations(frame)
# config['weightMapTuple'])
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)
desForceFrameBegin = getVal('des force begin')
desForceDuration = getVal('des force dur') * simulSpeedInv
desForceFrame = [
desForceFrameBegin, desForceFrameBegin + desForceDuration
]
desForceRelFrame = float(frame - desForceFrame[0]) / desForceDuration
desNormalForceMin = getVal('normal des force min')
desNormalForceMax = getVal('normal des force max')
desNormalForce = desNormalForceMin
if desForceFrame[0] <= frame <= desForceFrame[1]:
desNormalForce = desNormalForceMin * \
(1 - desForceRelFrame) + desNormalForceMax * desForceRelFrame
totalForce = np.array([0., desNormalForce, 0., 0., 0., 0.])
# totalForce = np.array([0., 720., 0., 0., 0., 0.])
# totalForce = np.array([50., 150.])
torques = None
ddth_des_flat[0:6] = [0.] * 6
self.setTimeStamp()
simulContactForces = np.zeros(3)
torque_None = True
for i in range(int(stepsPerFrame)):
torques = None
torque_None = True
cBodyIDs = []
cPositions = []
cPositionLocals = []
cForces = []
cBodyIDsControl = []
cPositionsControl = []
cPositionLocalsControl = []
cForcesControl = []
if desForceFrame[0] <= frame <= desForceFrame[1]:
if True:
# totalForceImpulse = stepsPerFrame * totalForce
cBodyIDs, cPositions, cPositionLocals, cForcesControl, torques \
= hls.calcLCPbasicControl(
motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, wForce, wTorque, ddth_des_flat)
# if cForces is not None:
# print "control: ", sum(cForces)
if torques is not None:
# print torques[:6]
torque_None = False
# cForcesControl = cForces.copy()
# cBodyIDsControl = cBodyIDs.copy()
# cPositionsControl = cPositions.copy()
# cPositionLocalsControl = cPositionLocals.copy()
else:
torques = ddth_des_flat
cBodyIDs, cPositions, cPositionLocals, cForces, timeStamp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
# if (not torque_None) and cForces is not None:
# print "calcul: ", sum(cForces)
if len(cBodyIDs) > 0:
# apply contact forces
if False and not torque_None:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals,
cForcesControl)
simulContactForces += sum(cForcesControl)
else:
vpWorld.applyPenaltyForce(cBodyIDs, cPositionLocals,
cForces)
simulContactForces += sum(cForces)
# simulContactForces += sum(cForces)
ype.nested(torques, torques_nested)
controlModel.setDOFTorques(torques_nested[1:])
vpWorld.step()
self.setTimeStamp()
# print ddth_des_flat
# print torques
print simulContactForces / stepsPerFrame
self.cBodyIDs, self.cPositions, self.cPositionLocals, self.cForces, torques \
= hls.calcLCPbasicControl(motion, vpWorld, controlModel, bodyIDsToCheck, 1., totalForce, wForce, wTorque, ddth_des_flat)
del rd_cForcesControl[:]
del rd_cPositionsControl[:]
for i in range(len(self.cBodyIDs)):
# print expected force
rd_cForcesControl.append(self.cForces[i].copy() / 50.)
rd_cPositionsControl.append(self.cPositions[i].copy())
del rd_ForceControl[:]
del rd_Position[:]
if self.cForces is not None:
# print expected force
rd_ForceControl.append(sum(self.cForces) / 50.)
rd_Position.append(np.array([0., 0., 0.1]))
# graph
if self.cForces is not None:
sumForce = sum(self.cForces)
viewer.cForceWnd.insertData('expForce', frame, sumForce[1])
else:
viewer.cForceWnd.insertData('expForce', frame, 0.)
self.cBodyIDs, self.cPositions, self.cPositionLocals, self.cForces, tmptmp \
= hls.calcLCPForces(motion, vpWorld, controlModel, bodyIDsToCheck, 1., torques, solver='qp')
del rd_cForces[:]
del rd_cPositions[:]
for i in range(len(self.cBodyIDs)):
# print calculated force
rd_cForces.append(self.cForces[i].copy() / 50.)
rd_cPositions.append(self.cPositions[i].copy())
del rd_jointPos[:]
for i in range(motion[0].skeleton.getJointNum()):
rd_jointPos.append(motion[frame].getJointPositionGlobal(i))
del rd_ForceDes[:]
del rd_PositionDes[:]
# rd_ForceDes.append(totalForce/50.)
rd_ForceDes.append(totalForce[1] * np.array([0., 1., 0.]) / 50.)
rd_PositionDes.append(np.array([0., 0., 0.]))
# if self.cForces is not None:
# rd_ForceDes.append(sum(self.cForces)[1]/50. * [0., 1., 0.])
# rd_PositionDes.append(np.array([0., 0., -0.1]))
# graph
if self.cForces is not None:
sumForce = sum(self.cForces)
# viewer.cForceWnd.insertData('realForce', frame, sumForce[1])
viewer.cForceWnd.insertData('realForce', frame,
simulContactForces[1] / stepsPerFrame)
else:
viewer.cForceWnd.insertData('realForce', frame, 0.)
viewer.cForceWnd.insertData('realForce', frame,
simulContactForces[1] / stepsPerFrame)
if desForceFrame[0] <= frame <= desForceFrame[1]:
viewer.cForceWnd.insertData('desForceMin', frame,
totalForce[1] * 1.)
# viewer.cForceWnd.insertData('desForceMin', frame, totalForce[1] * .9)
# viewer.cForceWnd.insertData('desForceMax', frame, totalForce[1] * 1.1)
else:
viewer.cForceWnd.insertData('desForceMin', frame, 0.)
viewer.cForceWnd.insertData('desForceMax', frame, 0.)
self.setTimeStamp()