(self)
return
Precomputes joint data to facilitating accessing data from motion frames.
Precomputes joint data to facilitating accessing data from motion frames.
Precomputes joint data to facilitating accessing data from motion frames .
def _build_joint_data(self):
    """Precomputes joint data to facilitating accessing data from motion frames."""
    num_joints = self._get_num_joints()
    self._joint_pose_idx = np.zeros(num_joints, dtype=np.int32)
    self._joint_pose_size = np.zeros(num_joints, dtype=np.int32)
    self._joint_vel_idx = np.zeros(num_joints, dtype=np.int32)
    self._joint_vel_size = np.zeros(num_joints, dtype=np.int32)

    for j in range(num_joints):
      pyb = self._get_pybullet_client()
      j_info = pyb.getJointInfo(self._ref_model, j)
      j_state = pyb.getJointStateMultiDof(self._ref_model, j)

      j_pose_idx = j_info[3]
      j_vel_idx = j_info[4]
      j_pose_size = len(j_state[0])
      j_vel_size = len(j_state[1])

      if (j_pose_idx < 0):
        assert (j_vel_idx < 0)
        assert (j_pose_size == 0)
        assert (j_vel_size == 0)

        if (j == 0):
          j_pose_idx = 0
          j_vel_idx = 0
        else:
          j_pose_idx = self._joint_pose_idx[j - 1] + self._joint_pose_size[j -
                                                                           1]
          j_vel_idx = self._joint_vel_idx[j - 1] + self._joint_vel_size[j - 1]

      self._joint_pose_idx[j] = j_pose_idx
      self._joint_pose_size[j] = j_pose_size
      self._joint_vel_idx[j] = j_vel_idx
      self._joint_vel_size[j] = j_vel_size

    motion = self.get_active_motion()
    motion_frame_size = motion.get_frame_size()
    motion_frame_vel_size = motion.get_frame_vel_size()
    pose_size = self.get_pose_size()
    vel_size = self.get_vel_size()
    assert (motion_frame_size == pose_size)
    assert (motion_frame_vel_size == vel_size)

    return
def _build_joint_data ( self ) : num_joints = self . _get_num_joints ( ) self . _joint_pose_idx = np . zeros ( num_joints , dtype = np . int32 ) self . _joint_pose_size = np . zeros ( num_joints , dtype = np . int32 ) self . _joint_vel_idx = np . zeros ( num_joints , dtype = np . int32 ) self . _joint_vel_size = np . zeros ( num_joints , dtype = np . int32 ) for j in range ( num_joints ) : pyb = self . _get_pybullet_client ( ) j_info = pyb . getJointInfo ( self . _ref_model , j ) j_state = pyb . getJointStateMultiDof ( self . _ref_model , j ) j_pose_idx = j_info [ 3 ] j_vel_idx = j_info [ 4 ] j_pose_size = len ( j_state [ 0 ] ) j_vel_size = len ( j_state [ 1 ] ) if ( j_pose_idx < 0 ) : assert ( j_vel_idx < 0 ) assert ( j_pose_size == 0 ) assert ( j_vel_size == 0 ) if ( j == 0 ) : j_pose_idx = 0 j_vel_idx = 0 else : j_pose_idx = self . _joint_pose_idx [ j - 1 ] + self . _joint_pose_size [ j - 1 ] j_vel_idx = self . _joint_vel_idx [ j - 1 ] + self . _joint_vel_size [ j - 1 ] self . _joint_pose_idx [ j ] = j_pose_idx self . _joint_pose_size [ j ] = j_pose_size self . _joint_vel_idx [ j ] = j_vel_idx self . _joint_vel_size [ j ] = j_vel_size motion = self . get_active_motion ( ) motion_frame_size = motion . get_frame_size ( ) motion_frame_vel_size = motion . get_frame_vel_size ( ) pose_size = self . get_pose_size ( ) vel_size = self . get_vel_size ( ) assert ( motion_frame_size == pose_size ) assert ( motion_frame_vel_size == vel_size ) return