Compute observation matching PrivPrivObservationsCfg structure
(sim_data, motion_loader, t, last_actions)
| 594 | |
| 595 | |
| 596 | def compute_observation(sim_data, motion_loader, t, last_actions): |
| 597 | """Compute observation matching PrivPrivObservationsCfg structure""" |
| 598 | # obs_dim = 554 |
| 599 | obs_dim = 554 - 42 - 84 |
| 600 | # obs_dim = 160 |
| 601 | USE_PRIVPRIV_OBS = False |
| 602 | # USE_PROJ_GRAV = False |
| 603 | USE_PROJ_GRAV = True |
| 604 | obs_dim = obs_dim + 42 + 84 if USE_PRIVPRIV_OBS else obs_dim |
| 605 | obs_dim = obs_dim + 3 if USE_PROJ_GRAV else obs_dim |
| 606 | |
| 607 | if t < 0: |
| 608 | return np.zeros(obs_dim, dtype=np.float32) |
| 609 | |
| 610 | obs = np.zeros(obs_dim, dtype=np.float32) |
| 611 | idx = 0 |
| 612 | |
| 613 | # 0. command (290,) - future joint pos + vel |
| 614 | command = get_command(motion_loader, t) |
| 615 | dim_command = command.shape[0] |
| 616 | obs[idx:idx + dim_command] = command |
| 617 | idx += dim_command |
| 618 | |
| 619 | # 1. motion_anchor_pos_b (15,) - future anchor pos in body frame |
| 620 | motion_anchor_pos = motion_anchor_pos_b_future(sim_data, motion_loader, t) |
| 621 | dim_motion_anchor_pos = motion_anchor_pos.shape[0] |
| 622 | obs[idx:idx + dim_motion_anchor_pos] = motion_anchor_pos |
| 623 | idx += dim_motion_anchor_pos |
| 624 | |
| 625 | # 2. motion_anchor_ori_b (30,) - future anchor ori in body frame |
| 626 | motion_anchor_ori = motion_anchor_ori_b_future(sim_data, motion_loader, t) |
| 627 | dim_motion_anchor_ori = motion_anchor_ori.shape[0] |
| 628 | obs[idx:idx + dim_motion_anchor_ori] = motion_anchor_ori |
| 629 | idx += dim_motion_anchor_ori |
| 630 | |
| 631 | # breakpoint() |
| 632 | if USE_PROJ_GRAV: |
| 633 | # 3. projected_gravity (3,) - projected gravity |
| 634 | projected_gravity = get_projected_gravity(sim_data) |
| 635 | dim_projected_gravity = projected_gravity.shape[0] |
| 636 | obs[idx:idx + dim_projected_gravity] = projected_gravity |
| 637 | idx += dim_projected_gravity |
| 638 | |
| 639 | # if True: |
| 640 | if USE_PRIVPRIV_OBS: |
| 641 | # 3. body_pos (42,) - robot body pos in body frame |
| 642 | body_pos = robot_body_pos_b(sim_data, motion_loader, t) |
| 643 | dim_body_pos = body_pos.shape[0] |
| 644 | obs[idx:idx + dim_body_pos] = body_pos |
| 645 | idx += dim_body_pos |
| 646 | |
| 647 | # 4. body_ori (84,) - robot body ori in body frame |
| 648 | body_ori = robot_body_ori_b(sim_data, motion_loader, t) |
| 649 | dim_body_ori = body_ori.shape[0] |
| 650 | obs[idx:idx + dim_body_ori] = body_ori |
| 651 | idx += dim_body_ori |
| 652 | |
| 653 | # 5. base_lin_vel (3,) - base linear velocity |
no test coverage detected