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)
