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()