def toPoint3d(tuple):
"""Converts a tuple to a Point3D. If it is already a Point3D, return it."""
from MathLib import Point3d, ThreeTuple
if tuple is None: return None
if isinstance(tuple, ThreeTuple): return Point3d(tuple)
if len(tuple) != 3:
raise TypeError("A Point3D should be a 3-tuple")
return Point3d(tuple[0], tuple[1], tuple[2])
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));
def _createEllipsoid(proxy, radius, colour, moiScale, withMesh):
"""
Private function.
Use createEllipsoid() or createArticulatedEllipsoid() instead.
"""
# Mesh and cdps will be set manually
proxy.meshes = None
proxy.cdps = None
# Compute box moi
moi = [0, 0, 0]
for i in range(3):
j = (i + 1) % 3
k = (i + 2) % 3
moi[i] = proxy.mass * (radius[j] * radius[j] +
radius[k] * radius[k]) / 5.0
proxy.moi = PyUtils.toVector3d(moi) * moiScale
ellipsoid = proxy.createAndFillObject()
cdp = Physics.SphereCDP()
cdp.setCenter(Point3d(0, 0, 0))
cdp.setRadius(min(radius))
ellipsoid.addCollisionDetectionPrimitive(cdp)
if withMesh:
mesh = Mesh.createEllipsoid((0, 0, 0), radius, colour)
ellipsoid.addMesh(mesh)
return ellipsoid
def _addHandle(self,
jointName,
componentIndex,
handleInfo=None,
type='circular',
reverseOppositeJoint=False,
minValue=-10000,
maxValue=10000):
"""
Adds a handle to the model.
handleInfo can be the name of an joint where the handle should be located
"""
oppositeJointName = jointName.replace("STANCE_", "TEMP_").replace(
"SWING_", "STANCE_").replace("TEMP_", "SWING_")
try:
handleJointName = handleInfo.replace("STANCE_",
"l").replace("SWING_", "r")
posInChild = self._character.getJointByName(
handleJointName).getParentJointPosition()
except AttributeError:
posInChild = Point3d(0, 0, 0)
handle = Handle(self._controller, jointName, componentIndex, type,
posInChild, oppositeJointName, reverseOppositeJoint,
minValue, maxValue)
self._handles.append(handle)
return handle
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 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 __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 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)
