def add_noise_to_pose_vel()

in env_humanoid_tracking.py [0:0]

• 46 lines of code
• 10 McCabe index (conditional complexity)

    def add_noise_to_pose_vel(self, agent, pose, vel=None, return_as_copied=True):
        '''
        Add a little bit of noise to the given pose and velocity
        '''

        ref_pose = copy.deepcopy(pose) if return_as_copied else pose
        if vel:
            ref_vel = copy.deepcopy(vel) if return_as_copied else vel
        dof_cnt = 0
        for j in agent._joint_indices:
            joint_type = agent.get_joint_type(j)
            ''' Ignore fixed joints '''
            if joint_type == self._pb_client.JOINT_FIXED:
                continue
            ''' Ignore if there is no corresponding joint '''
            if agent._char_info.bvh_map[j] == None:
                continue
            T = ref_pose.get_transform(agent._char_info.bvh_map[j], local=True)
            R, p = conversions.T2Rp(T)
            if joint_type == self._pb_client.JOINT_SPHERICAL:
                dR = math.random_rotation(
                    mu_theta=agent._char_info.noise_pose[j][0],
                    sigma_theta=agent._char_info.noise_pose[j][1],
                    lower_theta=agent._char_info.noise_pose[j][2],
                    upper_theta=agent._char_info.noise_pose[j][3])
                dof_cnt += 3
            elif joint_type == self._pb_client.JOINT_REVOLUTE:
                theta = math.truncnorm(
                    mu=agent._char_info.noise_pose[j][0],
                    sigma=agent._char_info.noise_pose[j][1],
                    lower=agent._char_info.noise_pose[j][2],
                    upper=agent._char_info.noise_pose[j][3])
                joint_axis = agent.get_joint_axis(j)
                dR = conversions.A2R(joint_axis*theta)
                dof_cnt += 1
            else:
                raise NotImplementedError
            T_new = conversions.Rp2T(np.dot(R, dR), p)
            ref_pose.set_transform(agent._char_info.bvh_map[j], T_new, do_ortho_norm=False, local=True)
            if vel is not None:
                dw = math.truncnorm(
                    mu=np.full(3, agent._char_info.noise_vel[j][0]),
                    sigma=np.full(3, agent._char_info.noise_vel[j][1]),
                    lower=np.full(3, agent._char_info.noise_vel[j][2]),
                    upper=np.full(3, agent._char_info.noise_vel[j][3]))
                ref_vel.data_local[j][:3] += dw
        return ref_pose, ref_vel