def processMouseMotion(self, event): """Process mouse motion within the window""" x = event.GetX() y = event.GetY() if self._cameraControl and self.HasCapture() and (self._oldX >= 0 or self._oldY >= 0): if event.LeftIsDown(): self._camera.modifyRotations( Vector3d((self._oldY - y) / 100.0, (self._oldX - x) / 100.0, 0)) if event.MiddleIsDown(): self._camera.translateCamDistance((self._oldY - y) / 200.0) if event.RightIsDown(): v = Vector3d((self._oldX - x) / 200.0, -(self._oldY - y) / 200.0, 0) camToWorld = TransformationMatrix() camToWorld.setToInverseCoordFrameTransformationOf( self._camera.getWorldToCam()) v = camToWorld * v self._camera.translateTarget(v) self._oldX = x self._oldY = y event.Skip()
def toVector3d(tuple): """Converts a tuple to a Vector3d. If it is already a ThreeTuple, return it as a vector.""" from MathLib import Vector3d, ThreeTuple if tuple is None: return None if isinstance(tuple, ThreeTuple): return Vector3d(tuple) if len(tuple) != 3: raise TypeError("A Vector3D should be a 3-tuple") return Vector3d(tuple[0], tuple[1], tuple[2])
def __init__(self, character, controller): """Initializes and attach to a standard Core.Character and Core.SimBiController.""" self._character = character self._controller = controller self._leftLowerLeg = self._character.getARBByName('lLowerLeg') self._rightLowerLeg = self._character.getARBByName('rLowerLeg') self._leftFoot = self._character.getARBByName('lFoot') self._rightFoot = self._character.getARBByName('rFoot') self._leftHipJointIndex = self._character.getJointIndex("lHip") self._rightHipJointIndex = self._character.getJointIndex("rHip") self._leftHipJointIndex = self._character.getJointIndex("lHip") self._rightHipJointIndex = self._character.getJointIndex("rHip") self._leftKneeJointIndex = self._character.getJointIndex("lKnee") self._rightKneeJointIndex = self._character.getJointIndex("rKnee") self._leftAnkleJointIndex = self._character.getJointIndex("lAnkle") self._rightAnkleJointIndex = self._character.getJointIndex("rAnkle") self._leftHipJoint = self._character.getJoint(self._leftHipJointIndex) self._rightHipJoint = self._character.getJoint( self._rightHipJointIndex) self._leftHipJoint = self._character.getJoint(self._leftHipJointIndex) self._rightHipJoint = self._character.getJoint( self._rightHipJointIndex) self._leftKneeJoint = self._character.getJoint( self._leftKneeJointIndex) self._rightKneeJoint = self._character.getJoint( self._rightKneeJointIndex) self._leftAnkle = self._character.getJoint(self._leftAnkleJointIndex) self._rightAnkle = self._character.getJoint(self._rightAnkleJointIndex) self._pelvis = self._leftHipJoint.getParent() self._leftUpperLeg = self._leftHipJoint.getChild() self._rightUpperLeg = self._rightHipJoint.getChild() self._leftAnkleInStanceLowerLeg = self._leftAnkle.getParentJointPosition( ) self._rightAnkleInStanceLowerLeg = self._rightAnkle.getParentJointPosition( ) self._leftHipJointInPelvis = self._leftHipJoint.getParentJointPosition( ) self._rightHipJointInPelvis = self._rightHipJoint.getParentJointPosition( ) self._leftHipJointInPelvis = self._leftHipJoint.getParentJointPosition( ) self._rightHipJointInPelvis = self._rightHipJoint.getParentJointPosition( ) self._leftHipToKneeVectorInUpperLeg = Vector3d( self._leftHipJoint.getChildJointPosition(), self._leftKneeJoint.getParentJointPosition()) self._rightHipToKneeVectorInUpperLeg = Vector3d( self._rightHipJoint.getChildJointPosition(), self._rightKneeJoint.getParentJointPosition()) self._leftKneeToAnkleVectorInLowerLeg = Vector3d( self._leftKneeJoint.getChildJointPosition(), self._leftAnkle.getParentJointPosition()) self._rightKneeToAnkleVectorInLowerLeg = Vector3d( self._rightKneeJoint.getChildJointPosition(), self._rightAnkle.getParentJointPosition())
def createEllipsoid(position=(0, 0, 0), radius=(1, 1, 1), colour=(0.6, 0.6, 0.6), samplesY=20, samplesXZ=20, exponentBottom=2, exponentTop=2, exponentSide=2): """ Creates the mesh for an ellipsoid having the specified position and radius Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ if exponentBottom < 2.0 or exponentTop < 2.0 or exponentSide < 2.0: raise ValueError('Exponents for ellipsoid must all be under 2.0!') position = PyUtils.toPoint3d(position) vertices = [] for i in range(1, samplesY): thetaI = i * math.pi / float(samplesY) if i < samplesY / 2: n = exponentTop else: n = exponentBottom cos = math.cos(thetaI) y = cos * radius[1] scaleXZ = math.pow(1 - math.pow(math.fabs(cos), n), 1.0 / float(n)) for j in range(0, samplesXZ): thetaJ = j * 2.0 * math.pi / float(samplesXZ) n = exponentSide cos = math.cos(thetaJ) x = cos * scaleXZ * radius[0] z = math.pow(1 - math.pow(math.fabs(cos), n), 1.0 / float(n)) * math.copysign( 1, math.sin(thetaJ)) * scaleXZ * radius[2] vertices.append(position + Vector3d(x, y, z)) vertices.append(position + Vector3d(0, radius[1], 0)) vertices.append(position + Vector3d(0, -radius[1], 0)) faces = [] for i in range(0, (samplesY - 2) * samplesXZ, samplesXZ): for j in range(0, samplesXZ): faces.append( (i + j, i + (j + 1) % samplesXZ, i + samplesXZ + (j + 1) % samplesXZ, i + samplesXZ + j)) for i in range(0, samplesXZ): base = (samplesY - 2) * samplesXZ faces.append(((i + 1) % samplesXZ, i, (samplesY - 1) * samplesXZ)) faces.append((base + i, base + (i + 1) % samplesXZ, (samplesY - 1) * samplesXZ + 1)) return create(vertices, faces, colour)
def simulationStep(self): """Performs a single simulation step""" # TODO Quite hacky if self._kinematicMotion: import KeyframeEditor from MathLib import Trajectory3dv try: pc = self._posableCharacter traj = self._stanceFootToSwingFootTrajectory except AttributeError: pc = self._posableCharacter = KeyframeEditor.PosableCharacter.PosableCharacter( self.getCharacter(0), self.getController(0)) traj = self._stanceFootToSwingFootTrajectory = Trajectory3dv() traj.addKnot(0, Vector3d(-0.13, 0, -0.4)) traj.addKnot(0.5, Vector3d(-0.13, 0.125, 0)) traj.addKnot(1, Vector3d(-0.13, 0, 0.4)) self._phase = 0 self._stance = Core.LEFT_STANCE stanceToSwing = traj.evaluate_catmull_rom(self._phase) if self._stance == Core.RIGHT_STANCE: stanceToSwing.x = stanceToSwing.x * -1 pc.updatePose(self._phase, stanceToSwing, self._stance, True) self._phase += 0.00069 if self._phase >= 1.0: self._phase = 0 if self._stance == Core.LEFT_STANCE: self._stance = Core.RIGHT_STANCE else: self._stance = Core.LEFT_STANCE return world = Physics.world() controllers = self._controllerList._objects contactForces = world.getContactForces() for controller in controllers: controller.performPreTasks(self._dt, contactForces) world.advanceInTime(self._dt) contactForces = world.getContactForces() for controller in controllers: if controller.performPostTasks(self._dt, contactForces): step = Vector3d(controller.getStanceFootPos(), controller.getSwingFootPos()) step = controller.getCharacterFrame().inverseRotate(step) v = controller.getV() phi = controller.getPhase() if self._printStepReport: print "step: %3.5f %3.5f %3.5f. Vel: %3.5f %3.5f %3.5f phi = %f" % ( step.x, step.y, step.z, v.x, v.y, v.z, phi)
def createCylinder(basePoint=(0, -1, 0), tipPoint=(0, 1, 0), radius=1.0, colour=(0.6, 0.6, 0.6), samples=20): """ Creates the mesh for a cylinder between the two specified points. Colour should be a 3-tuple (R,G,B) or a 4-tuple (R,G,B,A) """ basePoint = PyUtils.toPoint3d(basePoint) tipPoint = PyUtils.toPoint3d(tipPoint) baseToTipVector = Vector3d(basePoint, tipPoint) if baseToTipVector.isZeroVector(): raise ValueError( 'Invalid points for cylinder: base and tip are equal!') baseToTipUnitVector = baseToTipVector.unit() xUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(0, 0, 1)) if xUnitVector.length() < 0.5: xUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(0, -1, 0)) xUnitVector.toUnit() yUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(-1, 0, 0)) if yUnitVector.length() < 0.5: yUnitVector = baseToTipUnitVector.crossProductWith(Vector3d(0, 1, 0)) yUnitVector.toUnit() vertices = [] for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append(basePoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append(tipPoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) for i in range(samples): theta = i * 2 * math.pi / float(samples) vertices.append(basePoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) vertices.append(tipPoint + xUnitVector * math.cos(theta) * radius + yUnitVector * math.sin(theta) * radius) faces = [range(0, samples), range(samples, 2 * samples)] for i in range(0, 2 * samples, 2): base = 2 * samples size = 2 * samples faces.append((base + i, base + i + 1, base + (i + 3) % size, base + (i + 2) % size)) return create(vertices, faces, colour)
def _createCylinder(proxy, axis, basePos, tipPos, radius, colour, moiScale, withMesh): """ Private function. Use createCylinder() or createArticulatedCylinder() instead. """ if axis != 0 and axis != 1 and axis != 2: raise ValueError('Axis must be 0 for x, 1 for y or 2 for z.') # Mesh and cdps will be set manually proxy.meshes = None proxy.cdps = None # Compute box moi moi = [0, 0, 0] height = math.fabs(tipPos - basePos) for i in range(3): if i == axis: moi[i] = proxy.mass * radius * radius / 2.0 else: moi[i] = proxy.mass * (3 * radius * radius + height * height) / 12.0 ### HACK! moi[i] = max(moi[i], 0.01) proxy.moi = PyUtils.toVector3d(moi) * moiScale cylinder = proxy.createAndFillObject() basePoint = [0, 0, 0] tipPoint = [0, 0, 0] basePoint[axis] = basePos tipPoint[axis] = tipPos basePoint3d = PyUtils.toPoint3d(basePoint) tipPoint3d = PyUtils.toPoint3d(tipPoint) baseToTipVector3d = Vector3d(basePoint3d, tipPoint3d) if baseToTipVector3d.isZeroVector(): raise ValueError( 'Invalid points for cylinder: base and tip are equal!') baseToTipUnitVector3d = baseToTipVector3d.unit() if height <= radius * 2.0: cdp = Physics.SphereCDP() cdp.setCenter(basePoint3d + baseToTipVector3d * 0.5) cdp.setRadius(height / 2.0) else: cdp = Physics.CapsuleCDP() cdp.setPoint1(basePoint3d + baseToTipUnitVector3d * radius) cdp.setPoint2(tipPoint3d + baseToTipUnitVector3d * -radius) cdp.setRadius(radius) cylinder.addCollisionDetectionPrimitive(cdp) if withMesh: mesh = Mesh.createCylinder(basePoint, tipPoint, radius, colour) cylinder.addMesh(mesh) return cylinder
def beginShadows(self): """This method should be called before drawing shadows After calling this method, the application should draw any mesh for which it wants a shadow. Make sure you do not call glColor when drawing the meshes. Call endShadows() when complete """ # Setup constants n = Vector3d(0, 1, 0) # Ground normal d = Vector3d(-150, 200, 200) # Light direction # Draw a 1 on the stencil buffer everywhere on the ground glClear(GL_STENCIL_BUFFER_BIT) glEnable(GL_STENCIL_TEST) glStencilFunc(GL_ALWAYS, 1, 0xffffffff) glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE) GLUtils.GLUtils_drawGround(50, 5, 98) # Setup the stencil to write only on the ground glStencilFunc(GL_LESS, 0, 0xffffffff) glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE) # Offset the shadow polygons glPolygonOffset(-2.0, -2.0) glEnable(GL_POLYGON_OFFSET_FILL) # Draw semitransparent black shadows glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) glColor4f(0.0, 0.0, 0.0, 0.5) # Setup the ModelView matrix to a skewed & flattening projection glPushMatrix() dot = n.dotProductWith(d) mat = [ dot - n.x * d.x, -n.x * d.y, -n.x * d.z, 0, -n.y * d.x, dot - n.y * d.y, -n.y * d.z, 0, -n.z * d.x, -n.z * d.y, dot - n.z * d.z, 0, 0, 0, 0, dot ] glMultMatrixd(mat)
def setPos(self, pos): if self._type == _Type.circular : posPivot = self._jointChild.getWorldCoordinates( self._pivotPosInChild ) self._converter.project( posPivot ) v1 = Vector3d( posPivot, Point3d(*self._converter.to3d(self._previousMousePos) ) ).unit() v2 = Vector3d( posPivot, Point3d(*self._converter.to3d(pos) ) ).unit() cos = v1.dotProductWith(v2) sin = v1.crossProductWith(v2).dotProductWith(Vector3d(*self._converter.normal())) * self._sign theta = math.atan2(sin, cos) else: # _Type.perpendicular posPivot = self._jointChild.getWorldCoordinates( self._pivotPosInChild ) handlePos = self._jointChild.getWorldCoordinates( self._handlePosInChild ) vector = Vector3d( posPivot, handlePos ).unit().crossProductWith( Vector3d(*self._converter.normal()) ) vector2d = self._converter.to2d( vector ) theta = (vector2d[0]*(pos[0]-self._previousMousePos[0]) + vector2d[1]*(pos[1]-self._previousMousePos[1])) * self._sign * perpendicularSpeed index = self._handle.getIndexForTime(self._time) if index is None: return value = self._handle.getKeyframeValue(index) self._handle.setKeyframeValue(index, value+theta)
def performPostTasks(self, dt, contactForces): """Performs all the tasks that need to be done after the physics simulation step.""" step = Vector3d(self.getStanceFootPos(), self.getSwingFootPos()) step = self.getCharacterFrame().inverseRotate(step) phi = self.getPhase() if step.z > 0.7 : self.setPhase( 1.0 ) if super(WalkController,self).performPostTasks(dt, contactForces): v = self.getV() print "step: %3.5f %3.5f %3.5f. Vel: %3.5f %3.5f %3.5f. phi = %f\n" % (step.x, step.y, step.z, v.x, v.y, v.z, phi); self.setupParameters()
def setupParameters(self): """Called whenever parameters are setup""" self.getState(0).setDuration(Core.cvar.SimGlobals_stepTime) #now prepare the step information for the following step: footStart = self.getStanceFootPos().z sagittalPlaneFutureFootPos = footStart + self.defaultStepSize self.swingFootTrajectory.clear() self.setSagittalBalanceFootPlacement(1) # self.swingFootTrajectory.addKnot(0, Point3d(0, 0.04, self.getSwingFootPos().z - footStart)); # self.swingFootTrajectory.addKnot(0.5, Point3d(0, 0.05 + 0.1 + Core.cvar.SimGlobals_stepHeight, 0.5 * self.getSwingFootPos().z + sagittalPlaneFutureFootPos * 0.5 - footStart)); # self.swingFootTrajectory.addKnot(1, Point3d(0, 0.05 + 0, sagittalPlaneFutureFootPos - footStart)); self.swingFootTrajectory.addKnot(0, Point3d(0, 0.06 + 0.1, 0)) self.swingFootTrajectory.addKnot( 0.75, Point3d(0, 0.08 + 0.1 + Core.cvar.SimGlobals_stepHeight, 0.15)) self.swingFootTrajectory.addKnot( 1.0, Point3d(0, 0.08 + Core.cvar.SimGlobals_stepHeight / 2, 0.15)) # controller->swingFootTrajectory.addKnot(1, Point3d(0, 0.05, 0.27)); takeAStep = False idleMotion = False if self.doubleStanceMode and idleMotion: if random.random() < 0.2: Core.cvar.SimGlobals_upperBodyTwist = (random.random() - 0.5) elif random.random() < 0.2: self.doubleStanceMode = False Core.cvar.SimGlobals_desiredHeading += Core.cvar.SimGlobals_upperBodyTwist + ( random.random() - 0.5) Core.cvar.SimGlobals_upperBodyTwist = 0 takeAStep = True v = self.getV() print "v.x: %f, v.z: %f" % (v.x, v.z) if math.fabs(v.x) < 0.1 and \ math.fabs(v.z) < 0.05 and \ math.fabs(Core.cvar.SimGlobals_VDelSagittal) <= 0.1 and \ shouldComeToStop : if not self.doubleStanceMode: # check out the distance between the feet... fMidPoint = Vector3d(self.stanceFoot.getCMPosition(), self.swingFoot.getCMPosition()) errV = self.characterFrame.inverseRotate( self.doubleStanceCOMError) if errV.length() < 0.05 and fMidPoint.length( ) < 0.2 and fMidPoint.length() > 0.05: self.doubleStanceMode = True
def loadMany(dataModule, count, *args): """Loads 'count' instances of the object contained in the specified data module. See 'load' for more details on what is a dataModule. If they have a 'name' attribute, an underscore and a number starting at '_0' will be appended to each instance. If they have a 'pos' attribute, then the positions will be moved around. Returns a list of all the loaded object. """ from MathLib import Vector3d data = _getData(dataModule) result = [] for i in range(0, count): dataCopy = data try: dataCopy.name = data.name + "_" + str(i) except AttributeError: pass try: dataCopy.pos += Vector3d(10, 10, 10) * i except AttributeError: pass result.append(dataCopy.load(*args)) return result
def __init__(self, parent, id=wx.ID_ANY, pos=wx.DefaultPosition, size=wx.DefaultSize, style=0, name='glpane', attribList=(), fps=30): super(GLPanel, self).__init__(parent, id, pos, size, style, name, attribList) self._glInitialized = False width, height = self.GetSizeTuple() self._gluiTopLevelWindow = GLUITopLevelWindow(width, height, self) self._gluiSizer = GLUtils.GLUIBoxSizer(GLUtils.GLUI_VERTICAL) self._gluiTopLevelWindow.setSizer(self._gluiSizer) # # Set the event handlers. self.Bind(wx.EVT_ERASE_BACKGROUND, self.processEraseBackgroundEvent) self.Bind(wx.EVT_SIZE, self.processSizeEvent) self.Bind(wx.EVT_PAINT, self.processPaintEvent) self.Bind(wx.EVT_LEFT_DOWN, self.processMouseDown) self.Bind(wx.EVT_LEFT_UP, self.processMouseUp) self.Bind(wx.EVT_MIDDLE_DOWN, self.processMouseDown) self.Bind(wx.EVT_MIDDLE_UP, self.processMouseUp) self.Bind(wx.EVT_RIGHT_DOWN, self.processMouseDown) self.Bind(wx.EVT_RIGHT_UP, self.processMouseUp) self.Bind(wx.EVT_MOTION, self.processMouseMotion) self.Bind( wx.EVT_LEFT_DOWN, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onLeftDown)) self.Bind( wx.EVT_LEFT_UP, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onLeftUp)) self.Bind( wx.EVT_LEFT_DCLICK, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onLeftDClick)) self.Bind( wx.EVT_MIDDLE_DOWN, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onMiddleDown)) self.Bind( wx.EVT_MIDDLE_UP, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onMiddleUp)) self.Bind( wx.EVT_MIDDLE_DCLICK, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onMiddleDClick)) self.Bind( wx.EVT_RIGHT_DOWN, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onRightDown)) self.Bind( wx.EVT_RIGHT_UP, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onRightUp)) self.Bind( wx.EVT_RIGHT_DCLICK, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onRightDClick)) self.Bind( wx.EVT_MOTION, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onMotion)) self.Bind( wx.EVT_MOUSEWHEEL, lambda event: self.processGLUIMouseEvent( event, self._gluiTopLevelWindow.onMouseWheel)) self.Bind(wx.EVT_IDLE, self.processIdle) # Set-up starting state self._drawAxes = False self._printLoad = False self._drawGround = True self._groundTexture = None self._cameraControl = True self._fps = fps self._load = 0 self._loadWindow = [] self._loadWindowSize = math.ceil(fps) self._oldX = -1 self._oldY = -1 self._drawCallbacks = [] self._postDrawCallbacks = [] self._oncePerFrameCallbacks = [] self._timeOfNextFrame = 0 # Set-up initial camera self._camera = GLUtils.GLCamera() self._camera.setTarget(Point3d(0, 1, 0)) self._camera.modifyRotations(Vector3d(-0.18, -3.141592 / 2.0, 0)) self._cameraTargetFunction = None
def _create(self): """Creates the basic model class for the simple keyframe editor. Characters are always forced to left stance.""" app = wx.GetApp() try: self._controller = app.getController(0) except IndexError: return self._character = self._controller.getCharacter() handlesSide = [] handlesFront = [] self._handles = [] handlesSide.append( self._addHandle( 'SWING_Shoulder', 2, 'SWING_Elbow', minValue = -3.14, maxValue = 3.14 ) ) handlesSide.append( self._addHandle( 'STANCE_Shoulder', 2, 'STANCE_Elbow', minValue = -3.14, maxValue = 3.14 ) ) handlesSide.append( self._addHandle( 'SWING_Elbow', 0, reverseOppositeJoint = True, minValue = -2.8, maxValue = 0 ) ) handlesSide.append( self._addHandle( 'STANCE_Elbow', 0, type = 'reverseCircular', reverseOppositeJoint = True,minValue = 0, maxValue = 2.8 ) ) handlesSide.append( self._addHandle( 'SWING_Ankle', 0, 'SWING_ToeJoint' ) ) handlesSide.append( self._addHandle( 'STANCE_Ankle', 0, 'STANCE_ToeJoint' ) ) handlesSide.append( self._addHandle( 'pelvis_lowerback', 2, 'lowerback_torso', minValue = -1.2, maxValue = 1.2 ) ) handlesSide.append( self._addHandle( 'lowerback_torso', 2, 'torso_head', minValue = -1.2, maxValue = 1.2 ) ) handlesSide.append( self._addHandle( 'torso_head', 2, minValue = -1.2, maxValue = 1.2 ) ) handlesFront.append( self._addHandle( 'SWING_Shoulder', 1, 'SWING_Elbow', type = 'reverseCircular', reverseOppositeJoint = True, minValue = -2, maxValue = 2 ) ) handlesFront.append( self._addHandle( 'STANCE_Shoulder', 1, 'STANCE_Elbow', type = 'reverseCircular', reverseOppositeJoint = True, minValue = -2, maxValue = 2 ) ) handlesFront.append( self._addHandle( 'SWING_Shoulder', 0, type = 'reversePerpendicular', minValue = -3.3, maxValue = 3.3 ) ) handlesFront.append( self._addHandle( 'STANCE_Shoulder', 0, type = 'perpendicular', minValue = -3.3, maxValue = 3.3 ) ) handlesFront.append( self._addHandle( 'pelvis_lowerback', 1, 'lowerback_torso', reverseOppositeJoint = True, minValue = -1.2, maxValue = 1.2 ) ) handlesFront.append( self._addHandle( 'lowerback_torso', 1, 'torso_head', reverseOppositeJoint = True, minValue = -1.2, maxValue = 1.2 ) ) handlesFront.append( self._addHandle( 'torso_head', 1, reverseOppositeJoint = True, minValue = -1.2, maxValue = 1.2 ) ) stanceKneeHandle = Handle( self._controller, 'STANCE_Knee', 0, minValue = 0.1, maxValue = 2.2 ) self._handles.append( stanceKneeHandle ) swingFootHandleSagittal = BaseHandle( self._controller.getSwingFootTrajectoryDeltaSagittal() ) swingFootHandleCoronal = BaseHandle( self._controller.getSwingFootTrajectoryDeltaCoronal() ) swingFootHandleHeight = BaseHandle( self._controller.getSwingFootTrajectoryDeltaHeight() ) self._handles.append( swingFootHandleSagittal ) self._handles.append( swingFootHandleCoronal ) self._handles.append( swingFootHandleHeight ) self._editors = [] self._times = [ i / float(self._nbEditors-1) for i in range(self._nbEditors) ] for handle in self._handles: handle.forceKeyframesAt([0,1],self._times) for handle in self._handles: handle.enforceSymmetry() stanceFootToSwingFootTrajectory = Trajectory3dv() stanceFootToSwingFootTrajectory.addKnot(0,Vector3d(-0.2,0,-0.4)) stanceFootToSwingFootTrajectory.addKnot(0.5,Vector3d(-0.2,0.125,0)) stanceFootToSwingFootTrajectory.addKnot(1,Vector3d(-0.2,0,0.4)) glCanvasSize = wx.GetApp().getGLCanvas().GetSize() minWidth = glCanvasSize.x / self._nbEditors - 50 minHeight = glCanvasSize.y / 2 for i, time in enumerate(self._times) : posableCharacter = PosableCharacter(PyUtils.copy(self._character), self._controller) if i == 0 or i == self._nbEditors-1: checkBoxVisible = False else : checkBoxVisible = True editor = EditorWindow( self._container, posableCharacter = posableCharacter, handlesSide = handlesSide, handlesFront = handlesFront, stanceKneeHandle = stanceKneeHandle, swingFootHandleSagittal = swingFootHandleSagittal, swingFootHandleCoronal = swingFootHandleCoronal, swingFootHandleHeight = swingFootHandleHeight, time = time, controller = self._controller, stanceFootToSwingFootTrajectory = stanceFootToSwingFootTrajectory, minWidth = minWidth, minHeight = minHeight, checkBoxVisible = checkBoxVisible ) functor = SetKeyframeVisibilityFunctor(self,i) editor.addCheckBoxCallback( functor.execute ) self._sizer.add(editor, 0, GLUtils.GLUI_EXPAND ) self._editors.append(editor) self._container.getParent().layout()
def createCharacter(self): """Creates a 3D character that follows this description.""" from App import Proxys blue = (0.5, 0.6, 0.8, 1) green = (0.5, 0.8, 0.6, 1) red = (0.892, 0.716, 0.602, 1) gray = (0.5, 0.5, 0.5, 1) character = Core.Character() character.setName("Instant Character") massScale = 900 pelvisSizeY = self.getWaistPosY() - self.getHipPosY() pelvisBottomPos = -pelvisSizeY / 2.0 - self.getLegSizeY() * 0.1 pelvisTopPos = pelvisSizeY / 2.0 pelvisRadius = self.getPelvisDiameter() / 2.0 rootPosY = self.getHipPosY() + pelvisSizeY / 2.0 + 0.007 pelvis = PyUtils.RigidBody.createArticulatedTaperedBox( name="pelvis", size=(pelvisRadius * 2.0, pelvisSizeY * 1.5, pelvisRadius * 1.2), moiScale=3, exponentBottom=2, exponentSide=2, mass=-massScale, pos=(0, rootPosY, 0), colour=blue) character.setRoot(pelvis) totalLowerBackSizeY = self.getChestPosY() - self.getWaistPosY() lowerBackOffsetY = 0 #0.15 * totalLowerBackSizeY lowerBackSizeX = self.getTorsoDiameter() * 0.7 lowerBackSizeY = totalLowerBackSizeY - lowerBackOffsetY lowerBackSizeZ = lowerBackSizeX * 0.7 lowerback = PyUtils.RigidBody.createArticulatedTaperedBox( name="lowerBack", size=(lowerBackSizeX, lowerBackSizeY, lowerBackSizeZ), exponentTop=3, exponentBottom=2, exponentSide=2, mass=-massScale, colour=green) character.addArticulatedRigidBody(lowerback) joint = Proxys.BallInSocketJoint( name="pelvis_lowerback", posInParent=(0, pelvisSizeY / 2.0, 0), posInChild=(0, -lowerBackSizeY / 2.0 - lowerBackOffsetY, 0), swingAxis1=(1, 0, 0), twistAxis=(0, 0, 1), limits=(-1.6, 1.6, -1.6, 1.6, -1.6, 1.6)).createAndFillObject() joint.setParent(pelvis) joint.setChild(lowerback) character.addJoint(joint) totalTorsoSizeY = self.getShoulderPosY() - self.getChestPosY() torsoOffsetY = -0.2 * totalTorsoSizeY torsoSizeX = self.getTorsoDiameter() torsoSizeY = totalTorsoSizeY - torsoOffsetY torsoSizeZ = torsoSizeX * 0.6 torso = PyUtils.RigidBody.createArticulatedTaperedBox( name="torso", size=(torsoSizeX, torsoSizeY, torsoSizeZ), exponentTop=2.2, exponentBottom=4, exponentSide=3, mass=-massScale, colour=green) character.addArticulatedRigidBody(torso) joint = Proxys.BallInSocketJoint( name="lowerback_torso", posInParent=(0, lowerBackSizeY / 2.0, 0), posInChild=(0, -torsoSizeY / 2.0 - torsoOffsetY, 0), swingAxis1=(1, 0, 0), twistAxis=(0, 0, 1), limits=(-1.6, 1.6, -1.6, 1.6, -1.6, 1.6)).createAndFillObject() joint.setParent(lowerback) joint.setChild(torso) character.addJoint(joint) headOffsetY = self.getNeckSizeY() headSizeX = self.getHeadSizeX() headSizeY = self.getHeadSizeY() headSizeZ = self.getHeadSizeZ() head = PyUtils.RigidBody.createArticulatedEllipsoid( name="head", radius=(headSizeX / 2.0, headSizeY / 2.0, headSizeZ / 2.0), mass=-massScale, withMesh=False) character.addArticulatedRigidBody(head) head.addMeshObj( "data/StockMeshes/head.obj", Vector3d(0, -0.064, 0), Vector3d(headSizeX * 6.5, headSizeY * 4.6, headSizeZ * 5.5)) head.setColour(*red) head.addMesh( PyUtils.Mesh.createCylinder( basePoint=(0, -headSizeY / 2.0 - headOffsetY - torsoSizeY * 0.1, 0), tipPoint=(0, -headSizeY * 0.2, 0), radius=0.12 * headSizeX, colour=red)) joint = Proxys.BallInSocketJoint( name="torso_head", posInParent=(0, torsoSizeY / 2.0, 0), posInChild=(0, -headSizeY / 2.0 - headOffsetY, 0), swingAxis1=(1, 0, 0), twistAxis=(0, 0, 1), limits=(-1.6, 1.6, -1.6, 1.6, -1.6, 1.6)).createAndFillObject() joint.setParent(torso) joint.setChild(head) character.addJoint(joint) leftUpperArmSizeY = self.getShoulderPosY() - self.getElbowPosY(1) leftUpperArmDiameter = self.getUpperArmDiameter(1) lUpperArm = PyUtils.RigidBody.createArticulatedCylinder( name="lUpperArm", moiScale=3, axis=0, basePos=-leftUpperArmSizeY / 2.0, tipPos=leftUpperArmSizeY / 2.0, radius=leftUpperArmDiameter / 2.0, mass=-massScale, colour=green) character.addArticulatedRigidBody(lUpperArm) lUpperArm.addMesh( PyUtils.Mesh.createSphere((-leftUpperArmSizeY / 2.0, 0, 0), leftUpperArmDiameter * 0.65, colour=green)) lUpperArm.addMesh( PyUtils.Mesh.createSphere((leftUpperArmSizeY / 2.0, 0, 0), leftUpperArmDiameter * 0.5, colour=green)) joint = Proxys.BallInSocketJoint( name="lShoulder", posInParent=(torsoSizeX * 0.52, torsoSizeY * 0.32, 0), posInChild=(-leftUpperArmSizeY / 2.0, 0, 0), swingAxis1=(0, 0, 1), twistAxis=(1, 0, 0), limits=(-100, 100, -1.5, 1.5, -100, 100)).createAndFillObject() joint.setParent(torso) joint.setChild(lUpperArm) character.addJoint(joint) rightUpperArmSizeY = self.getShoulderPosY() - self.getElbowPosY(-1) rightUpperArmDiameter = self.getUpperArmDiameter(-1) rUpperArm = PyUtils.RigidBody.createArticulatedCylinder( name="rUpperArm", moiScale=3, axis=0, basePos=-rightUpperArmSizeY / 2.0, tipPos=rightUpperArmSizeY / 2.0, radius=rightUpperArmDiameter / 2.0, mass=-massScale, colour=green) character.addArticulatedRigidBody(rUpperArm) rUpperArm.addMesh( PyUtils.Mesh.createSphere((rightUpperArmSizeY / 2.0, 0, 0), rightUpperArmDiameter * 0.65, colour=green)) rUpperArm.addMesh( PyUtils.Mesh.createSphere((-rightUpperArmSizeY / 2.0, 0, 0), rightUpperArmDiameter * 0.5, colour=green)) joint = Proxys.BallInSocketJoint( name="rShoulder", posInParent=(-torsoSizeX * 0.52, torsoSizeY * 0.32, 0), posInChild=(rightUpperArmSizeY / 2.0, 0, 0), swingAxis1=(0, 0, 1), twistAxis=(1, 0, 0), limits=(-100, 100, -1.5, 1.5, -100, 100)).createAndFillObject() joint.setParent(torso) joint.setChild(rUpperArm) character.addJoint(joint) leftLowerArmSizeY = self.getElbowPosY(1) - self.getWristPosY(1) leftLowerArmDiameter = self.getLowerArmDiameter(1) lLowerArm = PyUtils.RigidBody.createArticulatedCylinder( name="lLowerArm", moiScale=3, axis=0, basePos=-leftLowerArmSizeY / 2.0, tipPos=leftLowerArmSizeY / 2.0, radius=leftLowerArmDiameter / 2.0, mass=-massScale, colour=red) character.addArticulatedRigidBody(lLowerArm) lLowerArm.addMesh( PyUtils.Mesh.createTaperedBox( position=(leftLowerArmSizeY * 0.5 + leftLowerArmDiameter * 0.8, 0, 0), size=(leftLowerArmDiameter * 1.6, leftLowerArmDiameter * 0.5, leftLowerArmDiameter * 1.15), colour=red)) joint = Proxys.HingeJoint(name="lElbow", posInParent=(leftUpperArmSizeY / 2.0, 0, 0), posInChild=(-leftLowerArmSizeY / 2.0, 0, 0), axis=(0, 1, 0), limits=(-2.7, 0)).createAndFillObject() joint.setParent(lUpperArm) joint.setChild(lLowerArm) character.addJoint(joint) rightLowerArmSizeY = self.getElbowPosY(-1) - self.getWristPosY(-1) rightLowerArmDiameter = self.getLowerArmDiameter(-1) rLowerArm = PyUtils.RigidBody.createArticulatedCylinder( name="rLowerArm", moiScale=3, axis=0, basePos=-rightLowerArmSizeY / 2.0, tipPos=rightLowerArmSizeY / 2.0, radius=rightLowerArmDiameter / 2.0, mass=-massScale, colour=red) character.addArticulatedRigidBody(rLowerArm) rLowerArm.addMesh( PyUtils.Mesh.createTaperedBox( position=(-rightLowerArmSizeY * 0.5 - rightLowerArmDiameter * 0.8, 0, 0), size=(rightLowerArmDiameter * 1.6, rightLowerArmDiameter * 0.5, rightLowerArmDiameter * 1.15), colour=red)) joint = Proxys.HingeJoint(name="rElbow", posInParent=(-rightUpperArmSizeY / 2.0, 0, 0), posInChild=(rightLowerArmSizeY / 2.0, 0, 0), axis=(0, -1, 0), limits=(-2.7, 0)).createAndFillObject() joint.setParent(rUpperArm) joint.setChild(rLowerArm) character.addJoint(joint) leftUpperLegSizeY = self.getHipPosY() - self.getKneePosY(1) leftUpperLegDiameter = self.getUpperLegDiameter(1) lUpperLeg = PyUtils.RigidBody.createArticulatedCylinder( name="lUpperLeg", axis=1, basePos=-leftUpperLegSizeY / 2.0, tipPos=leftUpperLegSizeY / 2.0, radius=leftUpperLegDiameter / 2.0, moiScale=4, mass=-massScale, colour=blue) character.addArticulatedRigidBody(lUpperLeg) lUpperLeg.addMesh( PyUtils.Mesh.createSphere((0, leftUpperLegSizeY / 2.0, 0), leftUpperLegDiameter * 0.5, colour=blue)) lUpperLeg.addMesh( PyUtils.Mesh.createSphere((0, -leftUpperLegSizeY / 2.0, 0), leftUpperLegDiameter * 0.5, colour=blue)) joint = Proxys.BallInSocketJoint( name="lHip", posInParent=(pelvisRadius * self.getLegRelativeAnchorX(1), -pelvisSizeY / 2.0, 0), posInChild=(0, leftUpperLegSizeY / 2.0, 0), swingAxis1=(1, 0, 0), twistAxis=(0, 0, 1), limits=(-1.3, 1.9, -1, 1, -1, 1)).createAndFillObject() joint.setParent(pelvis) joint.setChild(lUpperLeg) character.addJoint(joint) rightUpperLegSizeY = self.getHipPosY() - self.getKneePosY(-1) rightUpperLegDiameter = self.getUpperLegDiameter(-1) rUpperLeg = PyUtils.RigidBody.createArticulatedCylinder( name="rUpperLeg", axis=1, basePos=-rightUpperLegSizeY / 2.0, tipPos=rightUpperLegSizeY / 2.0, radius=rightUpperLegDiameter / 2.0, moiScale=4, mass=-massScale, colour=blue) character.addArticulatedRigidBody(rUpperLeg) rUpperLeg.addMesh( PyUtils.Mesh.createSphere((0, -rightUpperLegSizeY / 2.0, 0), rightUpperLegDiameter * 0.5, colour=blue)) rUpperLeg.addMesh( PyUtils.Mesh.createSphere((0, rightUpperLegSizeY / 2.0, 0), rightUpperLegDiameter * 0.5, colour=blue)) joint = Proxys.BallInSocketJoint( name="rHip", posInParent=(-pelvisRadius * self.getLegRelativeAnchorX(-1), -pelvisSizeY / 2.0, 0), posInChild=(0, rightUpperLegSizeY / 2.0, 0), swingAxis1=(1, 0, 0), twistAxis=(0, 0, 1), limits=(-1.3, 1.9, -1, 1, -1, 1)).createAndFillObject() joint.setParent(pelvis) joint.setChild(rUpperLeg) character.addJoint(joint) leftLowerLegSizeY = self.getKneePosY(1) - self.getAnklePosY(1) leftLowerLegDiameter = self.getLowerLegDiameter(1) lLowerLeg = PyUtils.RigidBody.createArticulatedCylinder( name="lLowerLeg", axis=1, basePos=-leftLowerLegSizeY / 2.0, tipPos=leftLowerLegSizeY / 2.0, radius=leftLowerLegDiameter / 2.0, moiScale=4, mass=-massScale, colour=blue) character.addArticulatedRigidBody(lLowerLeg) joint = Proxys.HingeJoint(name="lKnee", posInParent=(0, -leftUpperLegSizeY / 2.0, 0), posInChild=(0, leftLowerLegSizeY / 2.0, 0), axis=(1, 0, 0), limits=(0, 2.5)).createAndFillObject() joint.setParent(lUpperLeg) joint.setChild(lLowerLeg) character.addJoint(joint) rightLowerLegSizeY = self.getKneePosY(-1) - self.getAnklePosY(-1) rightLowerLegDiameter = self.getLowerLegDiameter(-1) rLowerLeg = PyUtils.RigidBody.createArticulatedCylinder( name="rLowerLeg", axis=1, basePos=-rightLowerLegSizeY / 2.0, tipPos=rightLowerLegSizeY / 2.0, radius=rightLowerLegDiameter / 2.0, moiScale=4, mass=-massScale, colour=blue) character.addArticulatedRigidBody(rLowerLeg) joint = Proxys.HingeJoint(name="rKnee", posInParent=(0, -rightUpperLegSizeY / 2.0, 0), posInChild=(0, rightLowerLegSizeY / 2.0, 0), axis=(1, 0, 0), limits=(0, 2.5)).createAndFillObject() joint.setParent(rUpperLeg) joint.setChild(rLowerLeg) character.addJoint(joint) leftFootSizeX = self.getFootSizeX(1) leftFootSizeY = self.getAnklePosY(1) - self.getGroundPosY() leftFootSizeZ = self.getFootSizeZ(1) * 0.75 lFoot = PyUtils.RigidBody.createArticulatedTaperedBox( name="lFoot", size=(leftFootSizeX, leftFootSizeY, leftFootSizeZ), exponentSide=20, groundCoeffs=(0.0005, 0.2), moiScale=3, mass=-massScale, colour=gray) character.addArticulatedRigidBody(lFoot) joint = Proxys.UniversalJoint( name="lAnkle", posInParent=(0, -leftLowerLegSizeY / 2.0, 0), posInChild=(0, leftFootSizeY / 2.0, -leftFootSizeZ * 0.33 + leftLowerLegDiameter / 2.0), parentAxis=(0, 0, 1), childAxis=(1, 0, 0), limits=(-0.75, 0.75, -0.75, 0.75)).createAndFillObject() joint.setParent(lLowerLeg) joint.setChild(lFoot) character.addJoint(joint) rightFootSizeX = self.getFootSizeX(-1) rightFootSizeY = self.getAnklePosY(-1) - self.getGroundPosY() rightFootSizeZ = self.getFootSizeZ(-1) * 0.75 rFoot = PyUtils.RigidBody.createArticulatedTaperedBox( name="rFoot", size=(rightFootSizeX, rightFootSizeY, rightFootSizeZ), exponentSide=20, groundCoeffs=(0.0005, 0.2), moiScale=3, mass=-massScale, colour=gray) character.addArticulatedRigidBody(rFoot) joint = Proxys.UniversalJoint( name="rAnkle", posInParent=(0, -rightLowerLegSizeY / 2.0, 0), posInChild=(0, rightFootSizeY / 2.0, -rightFootSizeZ * 0.33 + rightLowerLegDiameter / 2.0), parentAxis=(0, 0, -1), childAxis=(1, 0, 0), limits=(-0.75, 0.75, -0.75, 0.75)).createAndFillObject() joint.setParent(rLowerLeg) joint.setChild(rFoot) character.addJoint(joint) leftToesSizeX = leftFootSizeX leftToesSizeY = leftFootSizeY * 0.66 leftToesSizeZ = self.getFootSizeZ(1) - leftFootSizeZ lToes = PyUtils.RigidBody.createArticulatedTaperedBox( name="lToes", size=(leftToesSizeX, leftToesSizeY, leftToesSizeZ), exponentSide=20, groundCoeffs=(0.0005, 0.2), moiScale=3, mass=-massScale, colour=gray) character.addArticulatedRigidBody(lToes) joint = Proxys.HingeJoint( name="lToeJoint", posInParent=(0, (leftToesSizeY - leftFootSizeY) / 2.0 + 0.003, leftFootSizeZ / 2.0), posInChild=(0, 0, -leftToesSizeZ / 2.0), axis=(1, 0, 0), limits=(-0.52, 0.1)).createAndFillObject() joint.setParent(lFoot) joint.setChild(lToes) character.addJoint(joint) rightToesSizeX = rightFootSizeX rightToesSizeY = rightFootSizeY * 0.66 rightToesSizeZ = self.getFootSizeZ(-1) - rightFootSizeZ rToes = PyUtils.RigidBody.createArticulatedTaperedBox( name="rToes", size=(rightToesSizeX, rightToesSizeY, rightToesSizeZ), exponentSide=20, groundCoeffs=(0.0005, 0.2), moiScale=3, mass=-massScale, colour=gray) character.addArticulatedRigidBody(rToes) joint = Proxys.HingeJoint( name="rToeJoint", posInParent=(0, (rightToesSizeY - rightFootSizeY) / 2.0 + 0.003, rightFootSizeZ / 2.0), posInChild=(0, 0, -rightToesSizeZ / 2.0), axis=(1, 0, 0), limits=(-0.52, 0.1)).createAndFillObject() joint.setParent(rFoot) joint.setChild(rToes) character.addJoint(joint) return character
def updatePose(self, time, stanceFootToSwingFoot, stance=Core.LEFT_STANCE, dontMoveStanceAnkle=False): """Updates the pose of the character to match the one at the specified time. Always use left stance. stanceFootToSwingFoot is a Vector3d for the vector linking the stance foot to the swing foot """ # Setup the stance leg if stance == Core.LEFT_STANCE: stanceLowerLeg = self._leftLowerLeg stanceAnkleInStanceLowerLeg = self._leftAnkleInStanceLowerLeg stanceHipJointInPelvis = self._leftHipJointInPelvis stanceHipJointIndex = self._leftHipJointIndex swingHipJointInPelvis = self._rightHipJointInPelvis swingHipToKneeVectorInUpperLeg = self._rightHipToKneeVectorInUpperLeg swingKneeToAnkleVectorInLowerLeg = self._rightKneeToAnkleVectorInLowerLeg swingKneeJointIndex = self._rightKneeJointIndex swingHipJointIndex = self._rightHipJointIndex else: stanceLowerLeg = self._rightLowerLeg stanceAnkleInStanceLowerLeg = self._rightAnkleInStanceLowerLeg stanceHipJointInPelvis = self._rightHipJointInPelvis stanceHipJointIndex = self._rightHipJointIndex swingHipJointInPelvis = self._leftHipJointInPelvis swingHipToKneeVectorInUpperLeg = self._leftHipToKneeVectorInUpperLeg swingKneeToAnkleVectorInLowerLeg = self._leftKneeToAnkleVectorInLowerLeg swingKneeJointIndex = self._leftKneeJointIndex swingHipJointIndex = self._leftHipJointIndex if dontMoveStanceAnkle: finalStanceAnkleInWorld = stanceLowerLeg.getWorldCoordinates( stanceAnkleInStanceLowerLeg) pose = Core.ReducedCharacterStateArray() self._controller.updateTrackingPose(pose, time, stance) reducedCharacter = Core.ReducedCharacterState(pose) # Update stanceFootToSwingFoot, adding in the delta stanceFootToSwingFoot += self._controller.computeSwingFootDelta( time, stance) # Recenter and reorient the character pos = reducedCharacter.getPosition() pos.x = pos.z = 0 reducedCharacter.setPosition(pos) reducedCharacter.setOrientation(Quaternion()) self._character.setState(pose, 0, False) leftFootWorldOrientation = self._leftFoot.getOrientation() rightFootWorldOrientation = self._rightFoot.getOrientation() currentStanceAnkleInWorld = stanceLowerLeg.getWorldCoordinates( stanceAnkleInStanceLowerLeg) currentStanceAnkleInPelvis = self._pelvis.getLocalCoordinates( currentStanceAnkleInWorld) currentStanceHipToAnkleInPelvis = Vector3d(stanceHipJointInPelvis, currentStanceAnkleInPelvis) lengthStanceHipToAnkle = currentStanceHipToAnkleInPelvis.length() stanceHipJointInWorld = self._pelvis.getWorldCoordinates( stanceHipJointInPelvis) desiredStanceAnkleInWorld = Point3d(stanceFootToSwingFoot * -0.5) yLentgh2 = lengthStanceHipToAnkle*lengthStanceHipToAnkle - \ desiredStanceAnkleInWorld.x*desiredStanceAnkleInWorld.x - \ desiredStanceAnkleInWorld.z*desiredStanceAnkleInWorld.z if yLentgh2 <= 0: desiredStanceAnkleInWorld.y = stanceHipJointInWorld.y else: desiredStanceAnkleInWorld.y = stanceHipJointInWorld.y - math.sqrt( yLentgh2) desiredStanceAnkleInPelvis = self._pelvis.getLocalCoordinates( desiredStanceAnkleInWorld) desiredStanceHipToAnkleInPelvis = Vector3d(stanceHipJointInPelvis, desiredStanceAnkleInPelvis) currentStanceHipToAnkleInPelvis.toUnit() desiredStanceHipToAnkleInPelvis.toUnit() rot = Quaternion(currentStanceHipToAnkleInPelvis, desiredStanceHipToAnkleInPelvis) currQuat = reducedCharacter.getJointRelativeOrientation( stanceHipJointIndex) currQuat *= rot reducedCharacter.setJointRelativeOrientation(currQuat, stanceHipJointIndex) pos = reducedCharacter.getPosition() pos.y -= desiredStanceAnkleInWorld.y reducedCharacter.setPosition(pos) self._character.setState(pose, 0, False) # Setup the swing leg currentStanceAnkleInWorld = stanceLowerLeg.getWorldCoordinates( stanceAnkleInStanceLowerLeg) desiredSwingAnkleInWorld = currentStanceAnkleInWorld + stanceFootToSwingFoot targetInPelvis = self._pelvis.getLocalCoordinates( desiredSwingAnkleInWorld) qParent = Quaternion() qChild = Quaternion() Core.TwoLinkIK_getIKOrientations(swingHipJointInPelvis, targetInPelvis, Vector3d(-1, 0, 0), swingHipToKneeVectorInUpperLeg, Vector3d(-1, 0, 0), swingKneeToAnkleVectorInLowerLeg, qParent, qChild) reducedCharacter.setJointRelativeOrientation(qChild, swingKneeJointIndex) reducedCharacter.setJointRelativeOrientation(qParent, swingHipJointIndex) self._character.setState(pose, 0, False) leftAnkleLocalOrientation = self._leftLowerLeg.getOrientation( ).getInverse() * leftFootWorldOrientation rightAnkleLocalOrientation = self._rightLowerLeg.getOrientation( ).getInverse() * rightFootWorldOrientation reducedCharacter.setJointRelativeOrientation(leftAnkleLocalOrientation, self._leftAnkleJointIndex) reducedCharacter.setJointRelativeOrientation( rightAnkleLocalOrientation, self._rightAnkleJointIndex) if dontMoveStanceAnkle: delta = finalStanceAnkleInWorld - stanceLowerLeg.getWorldCoordinates( stanceAnkleInStanceLowerLeg) pos = reducedCharacter.getPosition() + delta reducedCharacter.setPosition(pos) self._character.setState(pose, 0, False)