niil >SrSSKrSSKrSSKJrJrJrJrJrJ r J r J r J r SSK Jr SSKrSSKrSSKrSSKJr SSKJr SSKJr SSKJr S r\R6"S S 55r\R:SSSS S S \R<S4Sj5r"SS\R@S9r!"SS\RD5r#\R:"SS\!55r$\R:"SS\!55r%\R:S\&S\S\RN4S\RN4Sj5r(\R:S\&S\\S\RN4S\RN4Sj5r)\R:S\&S\S\RT4S\RT4S j5r+\R:"S!S"\,55r-g)#z'Crowd objects/human controllers module.N) AnyCallableDictIterableListOptionalUnionSequenceText)logging)base_position_sensor)sensor)autonomous_object)object_controller_posc\\rSrSr%\\S'\\S'\\S'Sr\ \ R\S'Sr g)MovingObjectRecord position_keyagent_idradiusN last_position) __name__ __module__ __qualname____firstlineno__r __annotations__intfloatrrnpndarray__static_attributes__rh/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/crowd_controller.pyrrs% - -(,-"**%,r$rc 4[US5(d[SRU55eSn [R"S5n [ U5GH n UcUR XRS9n OUb [S5eUn UR XSS9n U S==S - ss'[RRX- 5nX:aU S ==S - ss'MxX:aU S ==S - ss'MUc%[R"S 5 U "U 5 XSS 4s $[US5(d[SU35eURU SSU SSU5nUcU S==S - ss'M[R"S5 U "U 5 XUS 4s $ [R"S5 U "U 5 W W SS4$)a;Sample valid start and target position reachable from start. Args: scene: a SceneBase instance implementing get_random_valid_position function. start: a 2-tuple (x, y) of start position. If specified, no start is sampled. start_circles: a list of circle specification. Each circle is specified as a tuple ((x, y), r) of a center (x, y) and radius r. If specified, start position is sampled from within one of the start_circles. target_circles: same as start_circle. If specified, target positions is sampled from within one of the start_circles. num_sampling_retries: a positive int, number of attempts to sample a start, target pair. min_wall_distance: a float, the minimum distance to a wall. min_goal_euclidean_distance: a positive float, the minimum distance between start and target. max_goal_euclidean_distance: a positive float, the maximum distance between start and target. min_path_clearance: float, clearance of shortest path to walls. Returns: A 4 tuple (start, target, shortest_path, is_valid). start and target are start and target positions, shortest_path is a list of 2-tuples specifying the shortest path from start to target, is_valid is bool specifying whether the start, target pair is valid. If min_path_clearance is not specified, then shortest_path is None. get_random_valid_positionzKIncompatible scene {}. Expected to have `get_random_valid_position` method.cdUR5Hup[R"SX5 M g)Nz %s: %d)itemsr info)countersnamevalues r%_print_counters5sample_start_target_position.._print_countersIs#~~'  ll:t+(r$cg)Nrrrr$r%.sample_start_target_position..Msar$N)inclusion_circleszEAt most one of the arguments start and start_circles can be not None.attemptsr& min_euclidean max_euclideanz3Valid goal with no minimum path clearance checking.Tfind_shortest_pathz+scene %s missing find_shortest_path method path_clearancez0Valid start/target with path clearance checking.zNo valid start/target found.F) hasattr ValueErrorformat collections defaultdictranger)r!linalgnormr r,r9)scenestart start_circlestarget_circlesnum_sampling_retriesmin_wall_distancemin_goal_euclidean_distancemax_goal_euclidean_distancemin_path_clearancer0sampling_counters_ start_pos target_poseuclidean_distance shortest_paths r%sample_start_target_positionrSsJ 3 4 4  &- !!,"--i8 % &a }11 2>i  ",- -i001=Jj!Q&! (>?7(A-(7(A-(! llHI'( D$ .. 5. / /  7w ? AA,,"1 z"1~'9;M()Q.) LLCD%& - 55W 'Z ,,-.#$ Je ++r$cJ\rSrSrSrS\-4S\\4SjjrSr \ S5r S\ S \4S jr S\ S \R4S jrS\ S \S \\\4S \R(4Sjr\R.S \S \\\4S S4Sj5r\R.S\ S \R(4Sj5rSSjrSrg)CrowdControllerzCrowd controller interface.z%snamescr[U5UlX l[UR5UlSUlg)zConstructor. Args: names: Name of instance (dynamic object or human). position_key_formatter: Formatter to convert name to position sensor name. rN)list_names_position_key_formatterlen _num_instance _current_time)selfrWposition_key_formatters r%__init__CrowdController.__init__s.u+DK#9 T[[)DDr$cnSUs=::aUR:dO [SURSUS35eg)Nrz&instance_id must be an integer in [0, z), got .)r]r= num_instancer_ instance_ids r%_validate_instance_id%CrowdController._validate_instance_idsG 0d00 0  243D3D2EF Q  !! 1r$cUR$)z&Returns the number of crowd instances.)r]r_s r%reCrowdController.num_instances   r$rgreturncBURU5 URU$)zReturns the name of instance.)rhrZrfs r% instance_nameCrowdController.instance_names{+ ;;{ ##r$c:URU5 [X5$)z6Returns the individual controller of certain instance.)rh_IndividualControllerrfs r%instance_controller#CrowdController.instance_controllers {+  33r$time_sec observationsc US:a6UR[R05 [RUlO1X R:a"X lURURU5 UR U5 UR U5$)aReturns action of specific instance given observation. This method is for _IndividualController. Args: instance_id: Identifier of an object in the crowd. 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. r)_recalculate_actionsr INIT_TIMEr^rh_get_action_of_instance)r_rgrurvs r%instance_get_action#CrowdController.instance_get_actionsw"!|  1 ; ;R@,66d && &#  2 2LA{+  ' ' 44r$Nc[S5e)4Calculates crowd command for all instances in crowd.z9_recalculate_actions() should be implemented by subclass.NotImplementedErrorr_rurvs r%rx$CrowdController._recalculate_actionss C EEr$c[S5e)0Returns calculated actions of specific instance.z<_get_action_of_instance() should be implemented by subclass.rrfs r%rz'CrowdController._get_action_of_instances F HHr$cAg)@Sets the scene for crowd controller to obtain scene information.Nr)r_rDs r% set_sceneCrowdController.set_scenes r$)r^rZr]r[rmN)rrrr__doc__POSITION_SENSOR_POSTFIXrr rarhpropertyrerrorControllerBasersr rrControllerOutputr{abcabstractmethodrxrzrr#rr$r%rUrUs#'+-D&D HTN !   $s$t$ 44!2!A!A4 55(-5sO5(9(J(J58EE+/c ?E?CEE HH!2!C!CHH r$rU) metaclassc^\rSrSrSrS\S\4SjrS\S\ \ \ 4S\ R4S jrS rg ) rrzHA utility class that wraps crowd controller in ControllerBase interface.crowd_controllerrgcX lXlg)zConstructor. Args: crowd_controller: The controller of crowd to which this instance belong. instance_id: Identifier of a crowd instance. N) _instance_id_crowd_controller)r_rrgs r%ra_IndividualController.__init__s$-r$rurvrmcNURRURX5$)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. )rr{rrs r% get_action _IndividualController.get_actions(  ! ! 5 5 8 33r$)rrN)rrrrrrUrrar rr rrrrr#rr$r%rrrrsCP..S.33sO3(9(J(J3r$rrc^\rSrSrSrSS\\\S\\\\4U4SjjjrS\S\ \ \ 4S S4S jr S \ S \R4S jrS rU=r$)StationaryControllerz@A crowd controller that places crowd objects at fixed positions.N positions orientationsc B>[TU]"S0UD6 Uc#[R"SUR-5n[ U5[ U5s=:XaUR:Xd3O [ SURS[ U5S[ U5S35eXlX lg)zConstructor. Args: positions: Fixed positions (3D points) of crowd instances. orientations: Fixed orientations in quaternion of crowd instances. **kwargs: Keyword arguments to pass on to base class. N))rrrr&z;positions and orientations should all have the same length z. Got len(positions) = z, len(orientations) = rdr) superrar!arrayrer\r= _positions _orientations)r_rrkwargs __class__s r%raStationaryController.__init__s GvXXo0A0AABl y>S. C$2C2C C  G    6s9~6FG!!$\!2 31 6 77  O%r$rurvrmcAAg)r~Nrrs r%rx)StationaryController._recalculate_actionss  r$rgcbURU5 URUURU04$)r)rhrrrfs r%rz,StationaryController._get_action_of_instances3 {+ ??; '););K)H" LLr$)rrN)rrrrrr r rrarr rrxrrrrzr# __classcell__rs@r%rrsH;?&0&Xhuo67&&0+/c ??C MM!2!C!CMMr$rc F^\rSrSrSrSrSrSrSrSr Sr S S S S \\\ \ \\S S \S 4S \ S\ \ \ \ S\ \ \ \ S\ \S\ \S\ S\ S\ S\ S\ S\S\S\ \S\\ \ \ 4S\4U4SjjjrS&SjrSrS\ S \\\4S!S 4S"jrS!\R24S#jrS'S$jrS%rU=r$)(OrcaControllerizA crowd controller that controls crowd instances using ORCA algorithm. Crowd instance will be initialized at a specified start position and move towards specified target position in a linear path while avoid collision with each other.  g?r:?g333333?NFTrtimestepstart_positionstarget_positionsuse_position_generator group_sizesr max_speed_mpstime_horizon_secobstacle_time_horizon_secneighbor_distance_m max_neighborsworkaround_erp_issuemoving_objects_pos_keymoving_objects_radiusendless_trajectoryc &>[TU]"S0UD6 UbUc U(deU(d\[U5[U5s=:XaUR:Xd3O [ SURS[U5S[U5S35eXlX`lXplXlXl Xl Xl X@l Xl SUl[U[ 5(aU/[U 5-n[U5[U 5:wa [ S5e[#X5VVs/sHunn[%USUS9PM snnUlSUlSUlUR(aSUlSUlOP[0R2"U[0R4S 9Ul[0R2"U[0R4S 9UlS UlUcS /OUUlSUlSUlSUl[AUR5HnUR(aS nOUR,USS 24nURBRE[GU5URURURURUR URS5nUU:XaMe UR&HfnURBRESURURUR UR URHURS5Ul%Mh Xl&gs snnf)aConstructor. Args: timestep: Timestep of simulation. start_positions: A list of position (x, y, z) for crowd instances as their starting position. target_positions: A list of position (x, y, z) for crowd instances as their target position. use_position_generator: a boolean, if True than the start and end positions are sampled. start_positions and target_positions must be None group_sizes: If set, then crowd is split in groups randomly, whose sizes are picked in random from this group_size list. In this way, the crowd simulator sumulaters clusters of objects moving around. radius: Radius of crowd instances. max_speed_mps: Maximum crowd instance speed. time_horizon_sec: Time horizon in second. obstacle_time_horizon_sec: Time horizon for static obstacle in second. neighbor_distance_m: Neighbor distance in meters. Instances closer than this distance are considered neighbors. max_neighbors: Max number of neighbors. workaround_erp_issue: There is an issue with pybullet constraint that the constraint is solved only 20% per timestep. Need to amplify position delta by 5x to workaround this issue. moving_objects_pos_key: Position observation key of moving objects not controlled by the ORCA controller. moving_objects_radius: Radius of moving objects. Should be a float, which applies to all moving objects, or a sequence of float, which should be of the same length as moving_objects_pos_key. endless_trajectory: Only valid if use_position_generator is True. Agent returns to starting point after reaching goal to achieve endless motion. **kwargs: Keyword arguments to pass on to base class. NzDstart_positions and target_positions should both have length equals z: len(start_positions) = z, len(target_positions) = rdzumoving_objects_radius should be either a float or a sequence of float with the same length as moving_objects_pos_key.)rrrdtypeFr&)rrr:r'r'r)'rrar\rer= _timestep_radius_max_speed_mps_time_horizon_sec_obstacle_time_horizon_sec_neighbor_distance_m_max_neighbors_use_position_generator_endless_trajectory_scene isinstancer zipr_moving_objects_paths _path_indices_start_positions_target_positionsr!rfloat64_already_initialized _group_sizes_current_positions_command_positions_command_orientationsrA_orcaaddAgenttuplerr_workaround_erp_issue)r_rrrrrrrrrrrrrrrrkeyistart_positionrobjrs r%raOrcaController.__init__.sd Gv  (-=-I "$ # !  !S)9%: Od>O>O O''()&&)/&:%;<''*+;'<&=Q @A A NL'-&?# 3'#9 1DK'// !#&'=#>? !S)?%@@  B CC 5MOMKC bHMODDKD ##"d#d hhbjjId!xx(8 Kd %D*2 D#D"D!%D4$$ % % %..q"1"u5$$    # #      ) ) ,,    h]]& ##ZZ((   # #    .. .. **    cl$"6sOsL c hUS/nSn[[USSUSS55HunupgU[R"[R"USUS- 5[R"USUS- 5-5- nXB:dU[ U5S- :dMwUR U5 SnM U$)Nrr'rr&r:) enumeraterr!sqrtsquarer\append)r_pathsubsample_stepsubsampled_path traveled_distrsts r%_subsample_pathOrcaController._subsample_pathsAwiOMs49d12h78 6Arww ))AaD1Q4K 299QqTAaD[#9 9;;m  '1D A +=q! 9 r$cURce[R"URS4[RS9Ul[R"URS4[RS9Ul/Ul/UlSupSn[RRUR5nSn[UR5GHn[URUUS9upxpXTS- :Xa0SupSn[RRUR5nO-UcUSS U4/nUSS U4/nOXSS U4/- nX(SS U4/- nUS- nU (d [S 5eXpR USS24'XR USS24'UR!U 5n URR#[R$"U [R&S95 URR#S5 GM" g) zURH(a,UR4USSS 2UR4U'U SSS 2n SnOXU :aOUS - nMmXR<U'UR4UUSS24nOUR,USSnXSS- n[R(R+U5[RJ"[R:5RL-nUU- nS n[OUU-URP5U-nURRSU[U55 GM URTHnUURVn URRURX[U SS55 URZc'URRSURXS 5 ONU URZ- UR\- nURRSURX[USS55 U R5Ul-M URR_5 [ [UR55HnURRaU5unnUU4URUSS24'[Rb"UURUS 4- UURUS4- 5nSS[Rd"US- 5[Rf"US- 54UR"U'M g)z1Calculates crowd command for all crowd instances.NTr:))r'r'r'rr)axisFg@rr&rrr)4rrryrrrrAr\rZrsetAgentPositionrcopyrrr!repeatr_last_target_recalculation_secr]rBrCrrSrrrrrrrrr[rsumrrfinfoepsmin_DEFAULT_MAX_SPEED_MPSsetAgentPrefVelocityrrrrrdoStepgetAgentPositionarctan2sincos)r_rurvrdistrNrPrrr agent_nameposition distancesmax_coverage_distanceindextarget_position goal_vectorgoal_vector_normgoal_unit_vectorkvvelocityrxyyaws r%rx#OrcaController._recalculate_actionsse ## '11 1    ''' --/%)!$...S%&! ##AuT-B-B1bqb5-I'JK'!% 5 5 : : URc [S5eURU5 UR(a6SnURUURU- nURUX#--nOURUR 5nX@R U04$)rzBAttempted to get action of instance before _recalculate_actions().g@)r RuntimeErrorrhrrrr)r_rgk_erpdelta_positioncommand_positions r%rz&OrcaController._get_action_of_instanceNs &  N PP {+ !!e  ! !+ .  ! !+ . /  ! !+ .1G G00=BBD 77 Db HHr$c 0URnUH8nURRUVs/sHn[U5PM sn5 M: URR 5 Xlgs snf![ a [R"S5 gf=f)rz\Scene does not implement vectorized_map property. Crowd agent cannot avoid static obstacles.N) vectorized_mapr addObstaclerprocessObstaclesrrr exception)r_rDpolygonspolygonpoints r%rOrcaController.set_scenecsF%%h' 'B'e 'BC jj!!#k C F EFFs"*A2A- +A2-A22 BB)rrrrrrrrrrrrrrrrrrrrrr)rr)rrrrr_DEFAULT_NEIGHBOR_DISTANCE_M_DEFAULT_MAX_NEIGHBORS_DEFAULT_RADIUS_Mr _DEFAULT_TIME_HORIZON_SEC"_DEFAULT_OBSTACLE_TIME_HORIZON_SECr rr boolrr r rarrrrrxrrrzrr#rrs@r%rrs"#!'*$ >B>B/4#''3 9)K#?1#'/1=N!%!H6H6 % 9:H6!(5/!:; H6 'tn H6 C= H6H6H6H6"'H6!H6H6!H6'tnH6#5(5/#9:H6 !H6H6T $#LcOcO+/c ?cO?CcOJI->>I* F Fr$rrgobject_factory.rmcAU"U0UD6$zCA wrapper that removes instance_id in default crowd object factory.r)rgr8argsrs r%uniform_object_factoryr<ps   ( ((r$object_factoriescRA[RRU5nU"U0UD6$r:)r!rr)rgr=r;rr8s r%random_object_factoryr?zs,99##$45.  ( ((r$rcAU"U0UD6$rr)rgrr;rs r%sensor_factoryrAs    r$c \rSrSrSrSS\S\S\4S\S\R4S\ \S\ R44S jjr \S \\R4S j5r\S \4S j5rS rg) CrowdBuilderiz$A helper class to construct a crowd.Nrecrowd_controller_factory.r8sensor_factoriesc /UlSn[U5Vs/sH oeSU--PM nnU"US9Ul[U5Hn[R"Xv[ -S9n/n U(a4UH.n U R [XjXvS-U R-S95 M0 U"UU4[U 5-URRU5S9n URR U 5 M gs snf) aConstructor. Args: num_instance: Number of autonomous objects in the crowd. crowd_controller_factory: A callable that returns a crowd controller object. object_factory: Callable that returns an autonomous object. sensor_factories: list of sensor callables. crowdz_%d)rW)r.rN)rgrr.)rgsensors controllerN) _objectsrA _controllerr BasePositionSensorrrrArrrs) r_rerDr8rEcrowd_id_prefixrrWposition_sensor add_sensorsr an_objects r%raCrowdBuilder.__init__sDMO27 2E F2EQuqy (2EE F/ei  mm9%#! GsC+rmcUR$)z/Returns list of AutonomousObjects in the crowd.)rJrks r% crowd_objectsCrowdBuilder.crowd_objectss ==r$cUR$)zReturns the crowd controller.)rKrks r%rCrowdBuilder.crowd_controllers   r$)rKrJr)rrrrrrrrUrAutonomousObjectrgeneric_sensorSensorrarrrSrr#rr$r%rCrCs,JN &&&&!)o)= >&&s$5$F$FFG && !#~/D/D*D!EF &&P T"3"D"DE   r$rC).rrr?typingrrrrrrr r r abslr dataclassesginnumpyr!&pybullet_envs.minitaur.envs_v2.sensorsr rrXpybullet_envs.minitaur.robotsrrr dataclassr configurableInfrSABCMetarUrrrrrrrWr<r?rYrAobjectrCrr$r%rfs- WWW HK;;! ---'+/3046736=@=?VV48],],@S Sl3-<<3>%M?%M%MPNF_NFNFb ))S"3"D"DDE)*::))))'8889;)*:: ))!!Xc6D6K6K7L.M!'5'<'<!!3633r$