<s> import vtk <EOL> import time <EOL> import numpy as np <EOL> from director import transformUtils <EOL> from director . timercallback import TimerCallback <EOL> from director import propertyset <EOL> from collections import OrderedDict <EOL> class OrbitController ( TimerCallback ) : <EOL> def __init__ ( self , view ) : <EOL> TimerCallback . __init__ ( self ) <EOL> self . view = view <EOL> self . orbitTime = <NUM_LIT> <EOL> def tick ( self ) : <EOL> speed = <NUM_LIT> / self . orbitTime <EOL> degrees = self . elapsed * speed <EOL> self . view . camera ( ) . Azimuth ( degrees ) <EOL> self . view . render ( ) <EOL> class Flyer ( TimerCallback ) : <EOL> def __init__ ( self , view ) : <EOL> TimerCallback . __init__ ( self ) <EOL> self . view = view <EOL> self . flyTime = <NUM_LIT:0.5> <EOL> self . startTime = <NUM_LIT:0.0> <EOL> self . maintainViewDirection = False <EOL> self . positionZoom = <NUM_LIT> <EOL> def getCameraCopy ( self ) : <EOL> camera = vtk . vtkCamera ( ) <EOL> camera . DeepCopy ( self . view . camera ( ) ) <EOL> return camera <EOL> def zoomTo ( self , newFocalPoint , newPosition = None ) : <EOL> self . interp = vtk . vtkCameraInterpolator ( ) <EOL> self . interp . AddCamera ( <NUM_LIT:0.0> , self . getCameraCopy ( ) ) <EOL> c = self . getCameraCopy ( ) <EOL> newFocalPoint = np . array ( newFocalPoint ) <EOL> oldFocalPoint = np . array ( c . GetFocalPoint ( ) ) <EOL> oldPosition = np . array ( c . GetPosition ( ) ) <EOL> if newPosition is None : <EOL> if self . maintainViewDirection : <EOL> newPosition = oldPosition + ( newFocalPoint - oldFocalPoint ) <EOL> else : <EOL> newPosition = oldPosition <EOL> newPosition += self . positionZoom * ( newFocalPoint - newPosition ) <EOL> c . SetFocalPoint ( newFocalPoint ) <EOL> c . SetPosition ( newPosition ) <EOL> c . SetViewUp ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:1.0> ] ) <EOL> self . interp . AddCamera ( <NUM_LIT:1.0> , c ) <EOL> self . startTime = time . time ( ) <EOL> self . start ( ) <EOL> def tick ( self ) : <EOL> elapsed = time . time ( ) - self . startTime <EOL> t = ( elapsed / float ( self . flyTime ) ) if self . flyTime > <NUM_LIT:0> else <NUM_LIT:1.0> <EOL> self . interp . InterpolateCamera ( t , self . view . camera ( ) ) <EOL> self . view . render ( ) <EOL> if t >= <NUM_LIT:1.0> : <EOL> return False <EOL> class CameraTracker ( object ) : <EOL> def __init__ ( self , view , targetFrame ) : <EOL> self . view = view <EOL> self . targetFrame = targetFrame <EOL> self . camera = view . camera ( ) <EOL> self . actions = [ ] <EOL> self . properties = propertyset . PropertySet ( ) <EOL> self . properties . connectPropertyChanged ( self . onPropertyChanged ) <EOL> self . setup ( ) <EOL> def getTargetPose ( self ) : <EOL> return transformUtils . poseFromTransform ( self . targetFrame . transform ) <EOL> def getTargetQuaternion ( self ) : <EOL> return self . getTargetPose ( ) [ <NUM_LIT:1> ] <EOL> def getTargetPosition ( self ) : <EOL> return np . array ( self . targetFrame . transform . GetPosition ( ) ) <EOL> def getCameraTransform ( self ) : <EOL> c = self . camera <EOL> return transformUtils . getLookAtTransform ( c . GetFocalPoint ( ) , c . GetPosition ( ) , c . GetViewUp ( ) ) <EOL> def getCameraToTargetTransform ( self , targetFrame ) : <EOL> targetToWorld = transformUtils . copyFrame ( targetFrame ) <EOL> cameraToWorld = self . getCameraTransform ( ) <EOL> cameraToTarget = transformUtils . concatenateTransforms ( [ cameraToWorld , targetToWorld . GetLinearInverse ( ) ] ) <EOL> focalDistance = np . linalg . norm ( np . array ( self . camera . GetFocalPoint ( ) ) - np . array ( self . camera . GetPosition ( ) ) ) <EOL> return cameraToTarget , focalDistance <EOL> def setCameraFocalPointToTarget ( self ) : <EOL> self . camera . SetFocalPoint ( self . getTargetPosition ( ) ) <EOL> self . view . render ( ) <EOL> def getProperties ( ) : <EOL> return self . properties <EOL> def setup ( self ) : <EOL> pass <EOL> def reset ( self ) : <EOL> pass <EOL> def update ( self ) : <EOL> pass <EOL> def onAction ( self , actionName ) : <EOL> pass <EOL> def getMinimumUpdateRate ( self ) : <EOL> return <NUM_LIT:0> <EOL> def onPropertyChanged ( self , propertySet , propertyName ) : <EOL> pass <EOL> class PositionTracker ( CameraTracker ) : <EOL> def setup ( self ) : <EOL> self . actions = [ '<STR_LIT>' ] <EOL> def onAction ( self , actionName ) : <EOL> if actionName == '<STR_LIT>' : <EOL> self . setCameraFocalPointToTarget ( ) <EOL> def reset ( self ) : <EOL> self . lastTargetPosition = self . getTargetPosition ( ) <EOL> self . lastTargetQuaternion = self . getTargetQuaternion ( ) <EOL> def update ( self ) : <EOL> newTargetPosition = self . getTargetPosition ( ) <EOL> delta = newTargetPosition - self . lastTargetPosition <EOL> followAxes = [ True , True , True ] <EOL> for i in xrange ( <NUM_LIT:3> ) : <EOL> if not followAxes [ i ] : <EOL> delta [ i ] = <NUM_LIT:0.0> <EOL> self . lastTargetPosition = newTargetPosition <EOL> c = self . camera <EOL> oldFocalPoint = np . array ( c . GetFocalPoint ( ) ) <EOL> oldPosition = np . array ( c . GetPosition ( ) ) <EOL> c . SetFocalPoint ( oldFocalPoint + delta ) <EOL> c . SetPosition ( oldPosition + delta ) <EOL> self . view . render ( ) <EOL> class LookAtTracker ( CameraTracker ) : <EOL> def update ( self ) : <EOL> self . setCameraFocalPointToTarget ( ) <EOL> def reset ( self ) : <EOL> pass <EOL> class OrbitTracker ( PositionTracker ) : <EOL> def setup ( self ) : <EOL> super ( OrbitTracker , self ) . setup ( ) <EOL> self . properties . addProperty ( '<STR_LIT>' , <NUM_LIT:20> , attributes = propertyset . PropertyAttributes ( minimum = <NUM_LIT:1> , maximum = <NUM_LIT:100> , singleStep = <NUM_LIT:1> ) ) <EOL> def update ( self ) : <EOL> super ( OrbitTracker , self ) . update ( ) <EOL> orbitTime = self . properties . getProperty ( '<STR_LIT>' ) <EOL> speed = <NUM_LIT> / orbitTime <EOL> degrees = self . dt * speed <EOL> self . view . camera ( ) . Azimuth ( degrees ) <EOL> self . view . render ( ) <EOL> def getMinimumUpdateRate ( self ) : <EOL> return <NUM_LIT> <EOL> class PositionOrientationTracker ( CameraTracker ) : <EOL> def storeTargetPose ( self ) : <EOL> self . lastTargetPosition = self . getTargetPosition ( ) <EOL> self . lastTargetQuaternion = self . getTargetQuaternion ( ) <EOL> def reset ( self ) : <EOL> self . storeTargetPose ( ) <EOL> targetToWorld = transformUtils . copyFrame ( self . targetFrame . transform ) <EOL> cameraToWorld = self . getCameraTransform ( ) <EOL> cameraToTarget = transformUtils . concatenateTransforms ( [ cameraToWorld , targetToWorld . GetLinearInverse ( ) ] ) <EOL> self . boomTransform = cameraToTarget <EOL> self . focalDistance = np . linalg . norm ( np . array ( self . camera . GetFocalPoint ( ) ) - np . array ( self . camera . GetPosition ( ) ) ) <EOL> def update ( self ) : <EOL> previousTargetFrame = transformUtils . transformFromPose ( self . lastTargetPosition , self . lastTargetQuaternion ) <EOL> self . storeTargetPose ( ) <EOL> cameraToTarget , focalDistance = self . getCameraToTargetTransform ( previousTargetFrame ) <EOL> targetToWorld = self . targetFrame . transform <EOL> cameraToWorld = transformUtils . concatenateTransforms ( [ cameraToTarget , targetToWorld ] ) <EOL> c = self . camera <EOL> focalPoint = cameraToWorld . TransformPoint ( [ self . focalDistance , <NUM_LIT:0> , <NUM_LIT:0> ] ) <EOL> focalPoint = targetToWorld . GetPosition ( ) <EOL> c . SetPosition ( cameraToWorld . GetPosition ( ) ) <EOL> c . SetFocalPoint ( focalPoint ) <EOL> self . view . render ( ) <EOL> class SmoothFollowTracker ( CameraTracker ) : <EOL> def getMinimumUpdateRate ( self ) : <EOL> return <NUM_LIT:30> <EOL> def setup ( self ) : <EOL> self . properties . addProperty ( '<STR_LIT>' , <NUM_LIT:0.5> , attributes = propertyset . PropertyAttributes ( decimals = <NUM_LIT:1> , minimum = <NUM_LIT:0.1> , maximum = <NUM_LIT:5> , singleStep = <NUM_LIT:0.1> ) ) <EOL> self . properties . addProperty ( '<STR_LIT>' , <NUM_LIT:15> , attributes = propertyset . PropertyAttributes ( decimals = <NUM_LIT:1> , minimum = <NUM_LIT:0.5> , maximum = <NUM_LIT> , singleStep = <NUM_LIT:1> ) ) <EOL> self . properties . addProperty ( '<STR_LIT>' , <NUM_LIT:10> , attributes = propertyset . PropertyAttributes ( minimum = - <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT:2> ) ) <EOL> self . properties . addProperty ( '<STR_LIT>' , <NUM_LIT:0> , attributes = propertyset . PropertyAttributes ( minimum = - <NUM_LIT> , maximum = <NUM_LIT> , singleStep = <NUM_LIT:10> ) ) <EOL> def reset ( self ) : <EOL> self . currentVelocity = np . array ( [ <NUM_LIT:0.0> , <NUM_LIT:0.0> , <NUM_LIT:0.0> ] ) <EOL> def update ( self ) : <EOL> if not self . targetFrame : <EOL> return <EOL> r = self . properties . getProperty ( '<STR_LIT>' ) <EOL> theta = np . radians ( <NUM_LIT> - self . properties . getProperty ( '<STR_LIT>' ) ) <EOL> phi = np . radians ( <NUM_LIT> - self . properties . getProperty ( '<STR_LIT>' ) ) <EOL> x = r * np . cos ( phi ) * np . sin ( theta ) <EOL> y = r * np . sin ( phi ) * np . sin ( theta ) <EOL> z = r * np . cos ( theta ) <EOL> c = self . camera <EOL> targetToWorld = self . targetFrame . transform <EOL> currentPosition = np . array ( c . GetPosition ( ) ) <EOL> desiredPosition = np . array ( targetToWorld . TransformPoint ( [ x , y , z ] ) ) <EOL> smoothTime = self . properties . getProperty ( '<STR_LIT>' ) <EOL> newPosition , self . currentVelocity = smoothDamp ( currentPosition , desiredPosition , self . currentVelocity , smoothTime , maxSpeed = <NUM_LIT:100> , deltaTime = self . dt ) <EOL> trackerToWorld = transformUtils . getLookAtTransform ( targetToWorld . GetPosition ( ) , newPosition ) <EOL> c . SetFocalPoint ( targetToWorld . GetPosition ( ) ) <EOL> c . SetPosition ( trackerToWorld . GetPosition ( ) ) <EOL> self . view . render ( ) <EOL> class CameraTrackerManager ( object ) : <EOL> def __init__ ( self ) : <EOL> self . target = None <EOL> self . targetFrame = None <EOL> self . trackerClass = None <EOL> self . camera = None <EOL> self . view = None <EOL> self . timer = TimerCallback ( ) <EOL> self . timer . callback = self . updateTimer <EOL> self . addTrackers ( ) <EOL> self . initTracker ( ) <EOL> def updateTimer ( self ) : <EOL> tNow = time . time ( ) <EOL> dt = tNow - self . tLast <EOL> if dt < self . timer . elapsed / <NUM_LIT> : <EOL> return <EOL> self . update ( ) <EOL> def setView ( self , view ) : <EOL> self . view = view <EOL> self . camera = view . camera ( ) <EOL> def setTarget ( self , obj ) : <EOL> if obj == self . target : <EOL> return <EOL> self . disableActiveTracker ( ) <EOL> if not obj : <EOL> return <EOL> self . target = obj <EOL> self . targetFrame = obj . getChildFrame ( ) <EOL> self . callbackId = self . targetFrame . connectFrameModified ( self . onTargetFrameModified ) <EOL> self . initTracker ( ) <EOL> def disableActiveTracker ( self ) : <EOL> if self . targetFrame : <EOL> self . targetFrame . disconnectFrameModified ( self . callbackId ) <EOL> self . target = None <EOL> self . targetFrame = None <EOL> self . trackerClass = None <EOL> self . initTracker ( ) <EOL> def update ( self ) : <EOL> tNow = time . time ( ) <EOL> dt = tNow - self . tLast <EOL> self . tLast = tNow <EOL> if self . activeTracker : <EOL> self . activeTracker . dt = dt <EOL> self . activeTracker . update ( ) <EOL> def reset ( self ) : <EOL> self . tLast = time . time ( ) <EOL> if self . activeTracker : <EOL> self . activeTracker . reset ( ) <EOL> def getModeActions ( self ) : <EOL> if self . activeTracker : <EOL> return self . activeTracker . actions <EOL> return [ ] <EOL> def onModeAction ( self , actionName ) : <EOL> if self . activeTracker : <EOL> self . activeTracker . onAction ( actionName ) <EOL> def getModeProperties ( self ) : <EOL> if self . activeTracker : <EOL> return self . activeTracker . properties <EOL> return None <EOL> def onTargetFrameModified ( self , frame ) : <EOL> self . update ( ) <EOL> def initTracker ( self ) : <EOL> self . timer . stop ( ) <EOL> self . activeTracker = self . trackerClass ( self . view , self . targetFrame ) if self . trackerClass else None <EOL> self . reset ( ) <EOL> self . update ( ) <EOL> if self . activeTracker : <EOL> minimumUpdateRate = self . activeTracker . getMinimumUpdateRate ( ) <EOL> if minimumUpdateRate > <NUM_LIT:0> : <EOL> self . timer . targetFps = minimumUpdateRate <EOL> self . timer . start ( ) <EOL> def addTrackers ( self ) : <EOL> self . trackers = OrderedDict ( [ <EOL> [ '<STR_LIT>' , None ] , <EOL> [ '<STR_LIT>' , PositionTracker ] , <EOL> [ '<STR_LIT>' , PositionOrientationTracker ] , <EOL> [ '<STR_LIT>' , SmoothFollowTracker ] , <EOL> [ '<STR_LIT>' , LookAtTracker ] , <EOL> [ '<STR_LIT>' , OrbitTracker ] , <EOL> ] ) <EOL> def setTrackerMode ( self , modeName ) : <EOL> assert modeName in self . trackers <EOL> self . trackerClass = self . trackers [ modeName ] <EOL> self . initTracker ( ) <EOL> def smoothDamp ( current , target , currentVelocity , smoothTime , maxSpeed , deltaTime ) : <EOL> '''<STR_LIT>''' <EOL> smoothTime = max ( <NUM_LIT> , smoothTime ) <EOL> num = <NUM_LIT> / smoothTime ; <EOL> num2 = num * deltaTime ; <EOL> num3 = <NUM_LIT:1.0> / ( <NUM_LIT:1.0> + num2 + <NUM_LIT> * num2 * num2 + <NUM_LIT> * num2 * num2 * num2 ) <EOL> num4 = current - target <EOL> num5 = target <EOL> num6 = maxSpeed * smoothTime <EOL> num4 = np . clip ( num4 , - num6 , num6 ) <EOL> target = current - num4 <EOL> num7 = ( currentVelocity + num * num4 ) * deltaTime <EOL> currentVelocity = ( currentVelocity - num * num7 ) * num3 <EOL> num8 = target + ( num4 + num7 ) * num3 <EOL> for i in xrange ( len ( current ) ) : <EOL> if ( num5 [ i ] - current [ i ] > <NUM_LIT:0.0> == num8 [ i ] > num5 [ i ] ) : <EOL> num8 [ i ] = num5 [ i ] <EOL> currentVelocity [ i ] = ( num8 [ i ] - num5 [ i ] ) / deltaTime <EOL> return num8 , currentVelocity <EOL> class RobotModelFollower ( object ) : <EOL> def __init__ ( self , view , robotModel , jointController ) : <EOL> self . view = view <EOL> self . robotModel = robotModel <EOL> self . jointController = jointController <EOL> self . followAxes = [ True , True , True ] <EOL> self . callbackId = None <EOL> def start ( self ) : <EOL> self . callbackId = self . robotModel . connectModelChanged ( self . onModelChanged ) <EOL> self . lastTrackPosition = np . array ( self . jointController . q [ : <NUM_LIT:3> ] ) <EOL> def stop ( self ) : <EOL> self . robotModel . disconnectModelChanged ( self . callbackId ) <EOL> def getCameraCopy ( self ) : <EOL> camera = vtk . vtkCamera ( ) <EOL> camera . DeepCopy ( self . view . camera ( ) ) <EOL> return camera <EOL> def onModelChanged ( self , model ) : <EOL> newTrackPosition = np . array ( self . jointController . q [ : <NUM_LIT:3> ] ) <EOL> delta = newTrackPosition - self . lastTrackPosition <EOL> for i in xrange ( <NUM_LIT:3> ) : <EOL> if not self . followAxes [ i ] : <EOL> delta [ i ] = <NUM_LIT:0.0> <EOL> self . lastTrackPosition = newTrackPosition <EOL> c = self . view . camera ( ) <EOL> oldFocalPoint = np . array ( c . GetFocalPoint ( ) ) <EOL> oldPosition = np . array ( c . GetPosition ( ) ) <EOL> c . SetFocalPoint ( oldFocalPoint + delta ) <EOL> c . SetPosition ( oldPosition + delta ) <EOL> self . view . render ( ) </s>
