nitSrSSKrSSKrSSKrSSKJrJrJrJrJ r J r J r SSK J r SSKrSSKrSSKrSrSr\ \R(\R(\\ \44rSr"SS \R.S 9r\R2"S S \55r\R2"S S\55r"SS\R85r\R<"SS55r\R2"SS\55r \R2"SS\ 55r!\R2"SS\55r"\R2"SS\55r#\R2"SS\55r$\R2"SS\55r%g)z-Module for controllers of autonomous objects.N)AnyDictOptionalSequenceTextTupleUnion)logginggg-C6?animation_frame_numbercV\rSrSrSr\R S\S\\ \ 4S\ 4Sj5r Sr g) ControllerBasea%Base class of object controllers. Controller is similar to a policy in that its output controls autonomous object just as policy output controls agent. To reflect this similarity, get_action(), the function that "commands" to autonomous object, is named similar to the counterpart in policy. time_sec observationsreturncg)aYReturns position, orientation and pose based on time and observations. Args: time_sec: Time since simulation reset in seconds. If time < 0, returns initial values and ignores observations. observations: A dict of all observations. Returns: Position, orientation and an extra info dict for robot joints, human skeletal pose, etc. N)selfrrs i/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/object_controller.py get_actionControllerBase.get_action#srN)__name__ __module__ __qualname____firstlineno____doc__abcabstractmethodfloatrrrControllerOutputr__static_attributes__rrrr r sA   #D#I 3C  rr ) metaclassc^\rSrSrSrS S\\S\\4SjjrS\S\\ \ 4S \ 4S jr S r g) StationaryController4z8Controller that keeps constant position and orientation.Nposition orientationc[R"UbUOS5Ul[R"UbUOS5Ulg)Nrrrrrr)nparray _position _orientation)rr'r(s r__init__StationaryController.__init__8s7XX(*>hINDN". LBDrtrrc8AAURUR04$)z&Returns constant position orientation.)r/r0rr3rs rrStationaryController.get_action?s  < >>4,,b 00r)r0r/)NN)rrrrrrr r1rrrr!rr"rrrr%r%4sR@,0.2B!%B$UOB11#D#I13C1rr%c t\rSrSrSr\R S4S\\S\S\S\ 4Sjjr S \S \ \ \ 4S \4S jrS rg)CircularMotionControllerGzeController for circular motion. The motion trajectory goes around a center in a circle in xy-plane. Fcenterradiusangular_velocityface_travel_directionc^[R"U5UlX lX0lX@lg)a,Constructor. Args: center: Center of circular motion, [x, y, z] in meters. radius: Radius of the circle in meters. angular_velocity: Angular velocity of motion, unit rad/s, e.g. pi means completing a circle in 2 sec. face_travel_direction: If True, object will face direction of motion. N)r-r._center_radius_angular_velocity_face_travel_direction)rr:r;r<r=s rr1!CircularMotionController.__init__Ns%88F#DLL-"7rr3rrc vA[SU5n[R"[R"URU-5[R "URU-5S/5UR -UR-nUR(aURU-[R"UR5[R-S- -n[R"SS[R "US- 5[R"US- 545nO[R"S5nX504$)zFReturns position on the circle based on time and constant orientation.rr+) maxr-r.cosrAsinr@r?rBsignpi)rr3rr'yawr(s rr#CircularMotionController.get_actionbs  C Axx &&* + &&* +   ||#ll+H ""  " "Q & ''$(( )BEE 1A 5 7cHHaBFF37ORVVC!G_EFkHH\*k " $$r)rAr?rBr@N)rrrrrr-rKrr boolr1rrrr!rr"rrrr8r8Gse*,-2 888"'8'+ 8(%%#D#I%3C%rr8c(\rSrSrSrSrSrSrSrSr g) PatrolRepeatModevzEEnums that defines trajectory repeat mode for patrol type controller.rr,rFrN) rrrrr NO_REPEATLOOP BACK_TRACKRESETr"rrrrPrPvs!M) $* %rrPc~\rSrSr%Sr\\S'\R\S'\R\S'\R\S'Sr g) PatrolSegmentDataz-A data class that describes a patrol segment. start_timestart_positionvelocityr(rN) rrrrrr __annotations__r-ndarrayr"rrrrXrXs15** JJzzrrXc\rSrSrSrSS\R SS4S\\\S\S \ S \ \\ 4S \ \S \ \\4 S jjr SrSrSrSrS\S\\ \4S\4SjrSrg)WayPointPatrolControllerz1Controller for patrolling along define waypoints.rT?Npoints yaw_angler= repeat_mode speed_mps time_pointsc [U[5(aUO[UUlX lX0lXPl[ U5S:a[S[ U535eUb(UR[RLa [S5eUR[RLdUR[RLaUnUbUnOUR[RLa[U5US/-nOUR[RLad[U5[[USS55-nUb=[U5[USUS[R"USSS25- -5-nO[!SURS 35e[R"U5nUR"SS:Xa:[R$"U[R&"UR"SS55nOUR"SS :wa [S 5eSn /UlUcK[+USSUSS5H4upUR-XU 5upUR(R/U 5 M6 OY[+USSUSSWSSUSS5H;uppUR1XU XU - -5upUR(R/U 5 M= UR(Vs/sHoR2PM snUlXlgs snf) a}Constructor. Args: points: List of waypoints, shape Nx3 or Nx2, N is number of points. yaw_angle: Yaw angle of the object in radians. face_travel_direction: If True, yaw angle 'zero' will be redefined to be object's travel direction. Setting yaw_angle to zero with face_travel_direction == True will results in object always facing its travel direction. Non-zero yaw_angle will cause additional yaw offsets. repeat_mode: Behavior of object after reaching the last way point in list. If the value is Text, it is converted to PatrolRepeatMode. speed_mps: Speed in meters per second. time_points: List of times associated with points. These times represent when the object should arrive at the associated waypoint. Optional, but if provided it must have the same length as 'points'. If 'speed_mps' is None, then 'time_points' will be used as-is and there is no maximum segment speed. If 'speed_mps' is also defined, then it serves as a maximum speed value and time points which would result in a segment speed above this value will be altered such that the maximum segment speed is 'speed_mps'. rFz*Need at least two points in 'points', got Nz.Time points are not compatible with LOOP mode.rr,z Repeat mode z is not supported yet.rRz!Expect 'points' to be Nx2 or Nx3.) isinstancerP_repeat _yaw_anglerB _speed_mpslen ValueErrorrTrSrVlistrUreversedr-r.NotImplementedErrorshapehstackzeros _segmentszip_get_patrol_segment_by_speedappend_get_patrol_segment_by_timerZ_segment_times _cycle_time)rrcrdr=rerfrgaugmented_pointsaugmented_time_pointsr3 from_pointto_pointsegment from_timeto_timels rr1!WayPointPatrolController.__init__s:$.k;K#L#LK)+6 LO"7O 6{Q  6s6{m D FF4<<3C3H3H#H G HH (222 (...  + ).. .f 3 )44 4fXfSbk-B(CC  !%[ 1 RKObhh{15b5?Q6R$RS T!U &< = ??xx 01a A% BHH%5%;%;A%>BD    "a ' : ;; ADN"% 3B !1!"!5#7 *66zQO  g&#7 7: 3B !1!"!5  $&;AB&?7A 2* 55jA67Y;N6OQ  g& 7A26@A<<@DAsK8cURX5upVURU5n[R"XC5(aSnO6XdU- - nURb"[R "URU/5n[R"SU5(aUn O[R "XCXh- -/5n [U[R"U5XX-US9n X4$)z;Returns a PatrolSegmentData for the given points and times.rrZr[r\r() _get_vector_get_orientationr-isclosermminrGrXr.) rrrrr unit_vectorlengthr(rf new_to_timers rrz4WayPointPatrolController._get_patrol_segment_by_times **:@K'' 4K zz'%%ii/0i  $FFDOOY78  zz!YkFFG&2D%EFGkxx +( !G  rcURX5upEURU5n[U[R"U5X@R -US9nX5UR - -nXx4$)zFReturns a PatrolSegmentData for the given points and a constant speed.r)rrrXr-r.rm) rrr current_timerrr(rtimes rrx5WayPointPatrolController._get_patrol_segment_by_speedsf**:@K'' 4Kxx +. !G 4??2 2D =rc[R"U5[R"U5- n[RRU5nUS:Xa[ SUSUS35eX4- nXT4$)z8Gets the unit vector and length of a from/to point pair.rz*Length of patrol segment equal to 0, from z to .)r-r.linalgnormro)rrrvectorrrs rr$WayPointPatrolController._get_vector(sk XXh "((:"6 6F YY^^F #F { )l$xj; <</K  rc UR(a[R"USUS5OSUR-n[R"SS[R "US- 5[R "US- 545nU$)z4Gets the orientation quaternion given a unit vector.r,rrF)rBr-arctan2rlr.rIrH)rrrLr(s rr)WayPointPatrolController._get_orientation4sm)) ::k!nk!n 5/0DOO DC((Aq"&&q/266#'?CDK rr3rrcA[SU5nXR:aIUR[RLa URO[ R "XR5nUR[R"URU5S- nURURXR- --nXCRR504$)z>Returns position on, and orientation along the patrol segment.rr,)rGr|rkrPrSr-fmodrvbisect bisect_rightr{r[r\rZr(copy)rr3rrr's rr#WayPointPatrolController.get_action=s  Aq A  #||/?/I/II4  ++,nnV001D1DaH1LMG!1!1Q9K9K5K!LL  ((--/ 33r)r|rBrkr{rvrmrl)rrrrrrPrSrr rNr rrr1rzrxrrrrr!rr"rrrr`r`s9#$-1-=-G-G,/8<Z0ZZ'+Z"#T)+ Z #5/ Z%Xe_5Zx <  44#D#I43C4rr`cD^\rSrSrSrS\\S\\4U4SjjrSrU=r $)LinearPatrolControlleriQz@Controller for patrolling along a line segment (back and forth).rrc J>[TU]"X/4S[R0UD6 g)zConstructor. Args: from_point: Starting point of motion, [x, y, z] in meters. to_point: Returning point of motion, [x, y, z] in meters. **kwargs: Keyword arguments to pass onto base class. reN)superr1rPrT)rrrkwargs __class__s rr1LinearPatrolController.__init__Us- Gj+!1!6!6rr) rrrrrrr r1r" __classcell__)rs@rrrQs(H #E? !%  rrc r\rSrSrSrSS\S\S\\S\\S\S\4 S jjr S \S \ \\ 4S \ 4S jr Srg)ChaseControllerifzBController for an object to chase another object at certain speed.self_key target_keyinitial_positioninitial_orientationrfverbosec[R"U5Ul[R"U5UlURUlUS::a[ SUS35eXPlXlX lX`l SUl g)aConstructor. Args: self_key: Observation dict key of position of object being controlled. target_key: Observation dict key of position of target object. initial_position: Initial position of the object. initial_orientation: Initial orientation of the object in xyzw quaternion. speed_mps: Speed in meters per second, always positive. verbose: If True, log details of get_action() calculation for debugging. rz,'speed_mps' should be a positive value, got rN) r-r._init_position_init_orientation_previous_orientationrorm _self_key _target_key_verbose _time_sec)rrrrrrfrs rr1ChaseController.__init__jst"((#34DXX&9:D!%!7!7DA~  8 1 E GGON!MDNrrrrc US:a<SUlURR5URR504$X RnX R nXC- S-nXR- nXlUR (ap[R"SSS9 [R"SURURX RUR X R XV5 SSS5 [RRU5nU[:a+UR5URR504$XW- nU[!UR"U-U5-U-n [R$"USUS5n [R&"SS[R("U S - 5[R*"U S - 545n XlXR504$!,(df  GN=f) zReturns position and orientation of the object being controlled. Args: time_sec: Time since simulation reset in seconds. If time < 0, returns initial values and ignores observations. observations: A dict of all observations. rr,r,rrRT) precisionsuppressz5t = %.1f, self %s: %s, target %s: %s, v: %s, dt %.1f.Nr,rF)rrrrrrrr- printoptionsr info_trr _EPS_DISTANCErrrmrr.rIrH) rrr self_positiontarget_position delta_vectordelta_tdistanceunit_delta_vector new_positionnew_yawnew_orientations rrChaseController.get_actions!|dn  % % ')?)?)D)D)F JJ 0M"#3#34O$3y@L'GN }} ??Q 6 LWW^^\..%A%%|4D4D'E!  , 7yy~~l+H-  ! ! #T%?%?%D%D%F JJ$/%DOOg,Ex(PP#$Ljj*1-/@/CDGhh1bffWq[&9266'A+;NOPO!0 --/ 33) 7 6s AG// G>)rrrrrmrrrN)r*r+rbF)rrrrrrrr rNr1rrr!rr"rrrrrfs}J 4=6B"%$ "*%%-UO     >.4 .4#D#I.43C.4rrcf\rSrSrSrSS\S\S\4SjjrS\4SjrS \S \ \ \ 4S \ 4S jr S rg)AnimationFrameControllerizEAn extra action controller to control playback of animation sequence.fpspause_between_repeat_secstart_time_secc6XlSUlX lX0lg)zConstructor. Args: fps: Frame per second of animation. pause_between_repeat_sec: Pause between repeat in second. start_time_sec: The time when animation starts to play. N)_fps _total_length_pause_between_repeat_sec_start_time_sec)rrrrs rr1!AnimationFrameController.__init__sID%=")r total_lengthc:US::a[SUS35eXlg)z"Sets total animation frame length.rz(Total number of frame must be >= 0, got rN)ror)rrs rset_total_length)AnimationFrameController.set_total_lengths+q  4\N! D FF%rrrrc|[SXR- 5n[XR-5nUR(aJUUR[UR UR-5--n[ X0RS- 5n[R"S5[R"S5[U04$)zReturns animation frame number with default position and orientation. Args: time_sec: Time since simulation reset in seconds. If time < 0, returns initial values and ignores observations. observations: A dict of all observations. rr,r*r+) rGrintrrrrr-r^ANIMATION_FRAME_NUMBER_KEY)rrrframes rr#AnimationFrameController.get_actions1h!5!556H 99$ %E    s4#A#ADII#MN NPe%++a/0e ::i "**\":"E=+ ++r)rrrrN)g$@rErE)rrrrrr r1rrrrrr!rr"rrrrrs[M"&14'* *% *). *$ *&3&+ +#D#I+3C+rrc\rSrSrSrSS\S\S\\S\\S\\S \S \4S jjrS r S r Sr Sr S\S\ \\4S\4SjrSrg)ConversationControlleriafController for an object that mimics conversational behavior. A controlled object is arrayed in a conversation about a center point. When a target object reaches a thresholded distance away from the center, the controlled object will face the target object and move away from the target's intended path along an orthogonal direction vector until it passes. Nrrr'r(conversation_centerproximity_thresholdrfc(XlX l[R"U=(d S5Ul[R"U=(d S5Ul[R"U=(d S5UlX`lXplSUl SUl g)a0Constructor. Args: self_key: Observation dict key of position of object being controlled. target_key: Observation dict key of position of target object. position: Initial position of the object. orientation: Initial orientation of the object in xyzw quaternion. conversation_center: Position of the center of the conversation group. proximity_threshold: The distance from the conversation center that the target must reach in order to prompt a response from the controlled object. speed_mps: Speed in meters per second, always positive. r*r+N) rrr-r.r/r0_conversation_center_proximity_thresholdrm_prev_target_position_wait_position)rrrr'r(rrrfs rr1ConversationController.__init__sl*N!XXh3)4DN!< =D ")<)I JD 3O!%DDrcl[R"X R- 5n[R"XR- 5nU[RR U5- n[R "XE5U-nX`R- nUR UUURUR5$)aGets the waiting position for the controlled object. This returns the position that the controlled object should move towards to create physical space such that the target object may pass through the conversation space. Args: self_position: The current position of the controlled object. target_position: The current position of the target object. Returns: An xyz position representing the waiting position that the controlled object should move towards to create space. )r-r.rrrdot _get_positionr)rrr target_path self_pathunit_target_pathprojected_points r_get_wait_position)ConversationController._get_wait_positions ((?-G-GGHK)C)CCDI"RYY^^K%@@ffY9 target with a minimum length of min_delta and a maximum distance of max_delta. Args: source_position: The current position of the controlled object. target_position: The target position to move to. min_delta: The minimum amount of distance to move. max_delta: The maximum amount of distance to move. Returns: An xyz position representing the next position to move to. r)r-rr)rrr min_delta max_deltarrrs rr$ConversationController._get_positionIs[($5BLyy~~l+H"-:l  "-:l 2L rcbXR- S-n[RRU5$)zGets the distance from the target to the conversation center point. Args: target_position: The target position. Returns: The scalar distance from the target position to the conversation center. r)rr-rr)rrrs r_get_target_distance_to_center5ConversationController._get_target_distance_to_centerms,$&?&??9LL 99>>, ''rr3rrcHURnURnUS:aX RnX RnUR U5nXpR :aOUR bBURXV5nURUU5nURUUSUR5nODURUUR5nURUURSUR5nX`l[R"USUSURS/5nX404$)Gets the position and orientation of the controlled object. Args: t: The current time step. observations: Dict containing sensor observations for current time step. Returns: The new position and orientation for the controlled object. rrEr,rF)r/r0rrrrrrrrrmrr-r.) rr3rr'r(rrtarget_distance wait_positions rr!ConversationController.get_action|s.~~H##K Av">>2m$%5%56o;;OLo 44 4 # # /// O ++   %%    OO ++   % %' %%  NN  OO  $3 xx!hqk4>>!3DEFH " $$r) rr0r/rrrrmrr)NNNrbg?)rrrrrrrr r1rrrrrrr!rr"rrrrrs,0.26:,/"%"%%UO  %-UO  %*  @#B*"H (:%:%#D#I:%3C:%rrc\rSrSrSrSr\SS4S\S\S\\S\ \ \\ 4S \ \S \ \ 4 S jjr S \ S \ \\4S\4SjrS\4SjrSrg)PauseIfCloseByWrapperizA controller wrapper that pauses controller if object is close to others. This wrapper works best if the underlying controller is time based. It is intended to be a simple way to stop agent when blocked and is not for reliable collision avoidance. rbNwrapped_controller self_pos_keyothers_pos_keyspause_distance self_yaw_keyactive_front_sectorcNXlSUlSUlSUlX l[ U5Ul[U[5(aU/[U5-n[U5[U5:wa [S5e[ U5Ul UbUc [S5eXPl X`l g)aConstructor. Args: wrapped_controller: The controller being wrapped. self_pos_key: Observation key of self position. others_pos_keys: Observation keys of others' positions. pause_distance: The distance limit before the controller pauses in meters. Can be a float value which applies to all objects specified in others_pos_keys or a Sequence of float values with the same length as others_pos_keys denoting the pause distance for each individual object in the same order in others_pos_keys. Default pause distance is one meter. self_yaw_key: Observation key of self yaw. Required if active_front_sector is specified. active_front_sector: If specified, it defines pie-shaped active region in front of controlled object. The pie-shaped area is symmetric about the forward direction of controlled object with it angle defined by this arg in radians, Only when other objects shows up in this region and pause distance requirement is met, pause is actived. rirNz=pause_distance and others_pos_keys must have the same length.zCself_yaw_key must be specified if active_front_sector is specified.) _controller_pause_start_t_shift_t _last_action _self_pos_keyrp_others_pos_keysrjr rnro_pause_distance _self_yaw_key_active_front_sector)rrrrr r r s rr1PauseIfCloseByWrapper.__init__s:*DDMD% 1D.%((&'#o*>>n >c/22  I KK/D&<+?  O QQ& 3rr3rrcUS:a:SUlSUlURRX5UlUR$UR U5(a"URS:aXlUR$URS:a(U=RXR- - slSUlURRXR- U5UlUR$)rrri)rrr rr _should_pauser5s rr PauseIfCloseByWrapper.get_actions 1uddm**55 d    ,''  q       ! 0000  **55 mm \+d   rc $XRSSn[URUR5Hup4XSSnXR- n[R R U5nURcXt:*s $XRSn[R"Xg- [R"[R"U5[R"U5/55n Xt:*=(a& [R"U 5URS- :s $ g)z/Determines whether the controller should pause.NrFrF)rrwrrr-rrrrrr.rHrIarccos) rrself_position_2dpos_keyr  position_2d vector_2drrLrs rr#PauseIfCloseByWrapper._should_pauses#$6$67;#& t33$5 )"1-k0i *h " " *))--.q1ffY)288RVVC["&&+4N+OP*? #!:!:Q!>> @$5 r) rr rrrrrrr)rrrrr_DEFAULT_PAUSE_DISTANCE_Mr rrr r rr1rrr!rrNrr"rrrrrs"7P%)-114(1414 ~ 14 E8E?23 14 TN 14$E?14f  #D#I 3C D4rr)&rrrenumtypingrrrrrrr abslr dataclassesginnumpyr- INIT_TIMErr^r!rABCMetar configurabler%r8EnumrP dataclassrXr`rrrrrrrrr-s3 DDD    RZZdCi@A5s{{41>11$+%~+%+%\ tyy *     q4~q4q4h5(P4nP4P4f*+~*+*+ZM%^M%M%`pNppr