def drawArrow(self, startPos, endPos, vector=None, lineWidth=.02):
if vector == None:
vector = [endPos[i] - startPos[i] for i in range(3)]
elif startPos == None:
startPos = [endPos[i] - vector[i] for i in range(3)]
length = mm.length(vector)
if length == 0.: return
glPushMatrix()
arrowT = mm.Rp2T(mm.getSO3FromVectors((length, 0, 0), vector),
startPos)
glMultMatrixf(arrowT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
# line + cone all parts
glePolyCone(
((0, 0, 0), (0, 0, 0), (length - triLength, 0, 0),
(length - triLength, 0, 0), (length, 0, 0), (length, 0, 0)), None,
(lineWidth / 2., lineWidth / 2., lineWidth / 2., triWidth / 2., 0,
0))
glPopMatrix()
def render(self):
for i in range(len(self.Rs)):
if self.Rs[i] != None and self.ps[i] != None:
T = mm.Rp2T(self.Rs[i], self.ps[i])
glPushMatrix()
glMultMatrixf(T.transpose())
ygh.drawCoordinate(self.totalColor, self.axisLength)
glPopMatrix()
def drawCircularArrow(self,
startPos,
endPos,
rotVec=None,
lineWidth=.02,
radius=.1):
if rotVec == None:
rotVec = [endPos[i] - startPos[i] for i in range(3)]
elif startPos == None:
startPos = [endPos[i] - rotVec[i] for i in range(3)]
length = mm.length(rotVec)
if length == 0.: return
glPushMatrix()
axisT = mm.Rp2T(mm.getSO3FromVectors((0, 0, length), rotVec), startPos)
glMultMatrixf(axisT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
# axis
# self.drawLine((0,0,0), (0,0,length))
glePolyCylinder(((0, 0, 0), (0, 0, 0), (0, 0, length), (0, 0, length)),
None, lineWidth / 4.)
# circular line part
# gleHelicoid( rToroid , startRadius , drdTheta , startZ , dzdTheta ,
# startXform , dXformdTheta , startTheta , sweepTheta )
sweepTheta = 2 * math.pi * length * mm.DEG
gleHelicoid(lineWidth / 2., radius, 0., 0., radius, None, None, 0.,
sweepTheta)
# cone part
glPushMatrix()
glRotatef(sweepTheta, 0, 0, 1)
glTranslatef(radius, 0, radius * (sweepTheta / 360.))
glRotatef(-90, 1, 0, 0)
glePolyCone(
((0, 0, 0), (0, 0, 0), (0, 0, triLength), (0, 0, triLength)), None,
(triWidth / 2., triWidth / 2., 0, 0))
glPopMatrix()
glPopMatrix()
def test_FramesRenderer_OrientationsRenderer():
frame0 = mm.I_SE3()
frame1 = mm.Rp2T(mm.exp(mm.v3(0, 1, 0), math.pi / 8.), (1, 0, 0))
viewer = ysv.SimpleViewer()
viewer.doc.addRenderer('frame0', FramesRenderer([frame0], (255, 0, 0)))
viewer.doc.addRenderer('frame1', FramesRenderer([frame1], (255, 0, 0)))
viewer.doc.addRenderer(
'orientation0',
OrientationsRenderer([mm.T2R(frame0)], [mm.T2p(frame0)],
(0, 255, 0)))
viewer.doc.addRenderer(
'orientation1',
OrientationsRenderer([mm.T2R(frame1)], [mm.T2p(frame1)],
(0, 255, 0)))
viewer.show()
Fl.run()
def draw2DArrow(self, startPos, endPos, vector=None, lineWidth=.02):
if vector == None:
vector = [endPos[i] - startPos[i] for i in range(3)]
elif startPos == None:
startPos = [endPos[i] - vector[i] for i in range(3)]
# glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
glDisable(GL_CULL_FACE)
glPushMatrix()
length = mm.length(vector)
arrowT = mm.Rp2T(mm.getSO3FromVectors((length, 0, 0), vector),
startPos)
glMultMatrixf(arrowT.transpose())
triWidth = lineWidth * 3
triLength = triWidth * 1.2
angles = [0, 90]
for angle in angles:
glRotatef(angle, 1, 0, 0)
# line part
glBegin(GL_QUADS)
glVertex3f(0, 0, lineWidth / 2)
glVertex3f(0, 0, -lineWidth / 2)
glVertex3f(length - triLength, 0, -lineWidth / 2)
glVertex3f(length - triLength, 0, +lineWidth / 2)
glEnd()
# triangle part
glBegin(GL_TRIANGLES)
glVertex3f(length - triLength, 0, triWidth / 2)
glVertex3f(length - triLength, 0, -triWidth / 2)
glVertex3f(length, 0, 0)
glEnd()
glPopMatrix()
glEnable(GL_CULL_FACE)
def test_slope_character():
bvhFilePath = '../samples/wd2_WalkSameSame01.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfgfile = open('../samples/' + 'mcfg', 'r')
mcfg = cPickle.load(mcfgfile)
mcfgfile.close()
frameTime = 1/30.
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
stepsPerFrame = 30
wcfg.timeStep = (frameTime)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
motionModel = cvm.VpMotionModel(vpWorld, motion[0], mcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.initializeHybridDynamics()
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1.]*len(bodyIDsToCheck)
Kt = 20.; Dt = 2*(Kt**.5)
Ks = 2000; Ds = 2*(Ks**.5)
rd_box = yr.BoxesRenderer([(5., .2, 3.)], [mm.Rp2T(mm.rotZ(0.1), (0,-.5,0))], (0,0,255), yr.POLYGON_LINE)
contactPositions= []
contactForces = []
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('contactPositions', yr.PointsRenderer(contactPositions, (0,255,0), yr.POINT_POINT))
viewer.doc.addRenderer('contactForces', yr.VectorsRenderer(contactForces, contactPositions,(0,255,0)))
viewer.doc.addRenderer('rd_box', rd_box)
viewer.setMaxFrame(500)
def simulateCallback(frame):
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)
for i in range(stepsPerFrame):
# get penalty forces
# bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, 1000, 10)
bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce_Boxes(rd_box.boxSizes, rd_box.Ts, bodyIDsToCheck, mus, 1000, 10)
# apply penalty forces
vpWorld.applyPenaltyForce(bodyIDs, positionLocals, forces)
controlModel.setDOFAccelerations(ddth_des)
controlModel.solveHybridDynamics()
vpWorld.step()
contactPositions[:] = positions
contactForces[:] = forces
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()
def test_slope_box():
bvhFilePath = '../samples/chain_1.bvh'
motion = yf.readBvhFile(bvhFilePath)
mcfg = ypc.ModelConfig()
mcfg.defaultDensity = 100.
mcfg.defaultBoneRatio = .8
for i in range(motion[0].skeleton.getElementNum()):
mcfg.addNode(motion[0].skeleton.getElementName(i))
wcfg = ypc.WorldConfig()
wcfg.planeHeight = 0.
wcfg.useDefaultContactModel = False
wcfg.gravity = (0,-9.8,0)
stepsPerFrame = 30
wcfg.timeStep = (1/30.)/stepsPerFrame
vpWorld = cvw.VpWorld(wcfg)
controlModel = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel2 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
controlModel3 = cvm.VpControlModel(vpWorld, motion[0], mcfg)
vpWorld.initialize()
controlModel.translateByOffset((0,1,0))
controlModel2.translateByOffset((0,1,.5))
controlModel3.translateByOffset((0,1,1.))
bodyIDsToCheck = range(vpWorld.getBodyNum())
mus = [1., .2, .0]
Ks = 1000; Ds = 2*(Ks**.5)
rd_box = yr.BoxesRenderer([(5., .2, 3.)], [mm.Rp2T(mm.rotZ(0.1), (0,-.5,0))], (0,0,255), yr.POLYGON_LINE)
contactPositions= []
contactForces = []
viewer = ysv.SimpleViewer()
# viewer.record(False)
# viewer.doc.addRenderer('motion', yr.JointMotionRenderer(motion, (0,0,255), yr.LINK_WIREBOX))
viewer.doc.addObject('motion', motion)
viewer.doc.addRenderer('model', cvr.VpModelRenderer(controlModel, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('model2', cvr.VpModelRenderer(controlModel2, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('model3', cvr.VpModelRenderer(controlModel3, (255,240,255), yr.POLYGON_LINE))
viewer.doc.addRenderer('contactPositions', yr.PointsRenderer(contactPositions, (0,255,0), yr.POINT_POINT))
viewer.doc.addRenderer('contactForces', yr.VectorsRenderer(contactForces, contactPositions,(0,255,0)))
viewer.doc.addRenderer('rd_box', rd_box)
viewer.setMaxFrame(500)
def simulateCallback(frame):
for i in range(stepsPerFrame):
# controlModel.applyBodyForceGlobal(0, (1,0,0))
# controlModel2.applyBodyForceGlobal(0, (1,0,0))
# controlModel3.applyBodyForceGlobal(0, (1,0,0))
# get penalty forces
# bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce(bodyIDsToCheck, mus, 1000, 10)
bodyIDs, positions, positionLocals, forces = vpWorld.calcPenaltyForce_Boxes(rd_box.boxSizes, rd_box.Ts, bodyIDsToCheck, mus, 1000, 10)
# apply penalty forces
vpWorld.applyPenaltyForce(bodyIDs, positionLocals, forces)
vpWorld.step()
contactPositions[:] = positions
contactForces[:] = forces
viewer.setSimulateCallback(simulateCallback)
viewer.startTimer(1/30.)
viewer.show()
Fl.run()