public PhysicsBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName = null)
        {
            var poseExtractor = new ArticulationBodyPoseExtractor(rootBody);

            m_PoseExtractor = poseExtractor;
            m_SensorName    = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName;
            m_Settings      = settings;

            var numJointExtractorObservations = 0;
            var articBodies = poseExtractor.Bodies;

            if (articBodies != null)
            {
                m_JointExtractors = new IJointExtractor[articBodies.Length - 1]; // skip the root
                for (var i = 1; i < articBodies.Length; i++)
                {
                    var jointExtractor = new ArticulationBodyJointExtractor(articBodies[i]);
                    numJointExtractorObservations += jointExtractor.NumObservations(settings);
                    m_JointExtractors[i - 1]       = jointExtractor;
                }
            }
            else
            {
                m_JointExtractors = new IJointExtractor[0];
            }

            var numTransformObservations = m_PoseExtractor.GetNumPoseObservations(settings);

            m_Shape = new[] { numTransformObservations + numJointExtractorObservations };
        }
Exemple #2
0
        public PhysicsBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName = null)
        {
            m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody);
            m_SensorName    = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName;
            m_Settings      = settings;

            var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses);

            m_Shape = new[] { numTransformObservations };
        }
Exemple #3
0
        /// <summary>
        ///  Construct a new PhysicsBodySensor
        /// </summary>
        /// <param name="rootBody"></param>
        /// <param name="settings"></param>
        /// <param name="sensorName"></param>
        public PhysicsBodySensor(Rigidbody rootBody, GameObject rootGameObject, PhysicsSensorSettings settings, string sensorName = null)
        {
            m_PoseExtractor = new RigidBodyPoseExtractor(rootBody, rootGameObject);
            m_SensorName    = string.IsNullOrEmpty(sensorName) ? $"PhysicsBodySensor:{rootBody?.name}" : sensorName;
            m_Settings      = settings;

            var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses);

            m_Shape = new[] { numTransformObservations };
        }
        public PhysicsBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName = null)
        {
            var poseExtractor = new ArticulationBodyPoseExtractor(rootBody);

            m_PoseExtractor = poseExtractor;
            m_SensorName    = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName;
            m_Settings      = settings;

            var numJointExtractorObservations = 0;

            m_JointExtractors = new List <IJointExtractor>(poseExtractor.NumEnabledPoses);
            foreach (var articBody in poseExtractor.GetEnabledArticulationBodies())
            {
                var jointExtractor = new ArticulationBodyJointExtractor(articBody);
                numJointExtractorObservations += jointExtractor.NumObservations(settings);
                m_JointExtractors.Add(jointExtractor);
            }

            var numTransformObservations = m_PoseExtractor.GetNumPoseObservations(settings);

            m_Shape = new[] { numTransformObservations + numJointExtractorObservations };
        }
        /// <summary>
        /// Construct a new PhysicsBodySensor
        /// </summary>
        /// <param name="poseExtractor"></param>
        /// <param name="settings"></param>
        /// <param name="sensorName"></param>
        public PhysicsBodySensor(
            RigidBodyPoseExtractor poseExtractor,
            PhysicsSensorSettings settings,
            string sensorName
            )
        {
            m_PoseExtractor = poseExtractor;
            m_SensorName    = sensorName;
            m_Settings      = settings;

            var numJointExtractorObservations = 0;

            m_JointExtractors = new List <IJointExtractor>(poseExtractor.NumEnabledPoses);
            foreach (var rb in poseExtractor.GetEnabledRigidbodies())
            {
                var jointExtractor = new RigidBodyJointExtractor(rb);
                numJointExtractorObservations += jointExtractor.NumObservations(settings);
                m_JointExtractors.Add(jointExtractor);
            }

            var numTransformObservations = m_PoseExtractor.GetNumPoseObservations(settings);

            m_Shape = new[] { numTransformObservations + numJointExtractorObservations };
        }
Exemple #6
0
        /// <summary>
        ///  Construct a new PhysicsBodySensor
        /// </summary>
        /// <param name="rootBody">The root Rigidbody. This has no Joints on it (but other Joints may connect to it).</param>
        /// <param name="rootGameObject">Optional GameObject used to find Rigidbodies in the hierarchy.</param>
        /// <param name="virtualRoot">Optional GameObject used to determine the root of the poses,
        /// <param name="settings"></param>
        /// <param name="sensorName"></param>
        public PhysicsBodySensor(
            Rigidbody rootBody,
            GameObject rootGameObject,
            GameObject virtualRoot,
            PhysicsSensorSettings settings,
            string sensorName = null
            )
        {
            var poseExtractor = new RigidBodyPoseExtractor(rootBody, rootGameObject, virtualRoot);

            m_PoseExtractor = poseExtractor;
            m_SensorName    = string.IsNullOrEmpty(sensorName) ? $"PhysicsBodySensor:{rootBody?.name}" : sensorName;
            m_Settings      = settings;

            var numJointExtractorObservations = 0;
            var rigidBodies = poseExtractor.Bodies;

            if (rigidBodies != null)
            {
                m_JointExtractors = new IJointExtractor[rigidBodies.Length - 1]; // skip the root
                for (var i = 1; i < rigidBodies.Length; i++)
                {
                    var jointExtractor = new RigidBodyJointExtractor(rigidBodies[i]);
                    numJointExtractorObservations += jointExtractor.NumObservations(settings);
                    m_JointExtractors[i - 1]       = jointExtractor;
                }
            }
            else
            {
                m_JointExtractors = new IJointExtractor[0];
            }

            var numTransformObservations = m_PoseExtractor.GetNumPoseObservations(settings);

            m_Shape = new[] { numTransformObservations + numJointExtractorObservations };
        }
Exemple #7
0
        /// <summary>
        /// Utility method for writing a PoseExtractor to an ObservationWriter.
        /// </summary>
        /// <param name="writer"></param>
        /// <param name="settings"></param>
        /// <param name="poseExtractor"></param>
        /// <param name="baseOffset">The offset into the ObservationWriter to start writing at.</param>
        /// <returns>The number of observations written.</returns>
        public static int WritePoses(this ObservationWriter writer, PhysicsSensorSettings settings, PoseExtractor poseExtractor, int baseOffset = 0)
        {
            var offset = baseOffset;

            if (settings.UseModelSpace)
            {
                foreach (var pose in poseExtractor.GetEnabledModelSpacePoses())
                {
                    if (settings.UseModelSpaceTranslations)
                    {
                        writer.Add(pose.position, offset);
                        offset += 3;
                    }

                    if (settings.UseModelSpaceRotations)
                    {
                        writer.Add(pose.rotation, offset);
                        offset += 4;
                    }
                }

                foreach (var vel in poseExtractor.GetEnabledModelSpaceVelocities())
                {
                    if (settings.UseModelSpaceLinearVelocity)
                    {
                        writer.Add(vel, offset);
                        offset += 3;
                    }
                }
            }

            if (settings.UseLocalSpace)
            {
                foreach (var pose in poseExtractor.GetEnabledLocalSpacePoses())
                {
                    if (settings.UseLocalSpaceTranslations)
                    {
                        writer.Add(pose.position, offset);
                        offset += 3;
                    }

                    if (settings.UseLocalSpaceRotations)
                    {
                        writer.Add(pose.rotation, offset);
                        offset += 4;
                    }
                }

                foreach (var vel in poseExtractor.GetEnabledLocalSpaceVelocities())
                {
                    if (settings.UseLocalSpaceLinearVelocity)
                    {
                        writer.Add(vel, offset);
                        offset += 3;
                    }
                }
            }

            return(offset - baseOffset);
        }
        /// <summary>
        /// Utility method for writing a PoseExtractor to an ObservationWriter.
        /// </summary>
        /// <param name="writer"></param>
        /// <param name="settings"></param>
        /// <param name="poseExtractor"></param>
        /// <param name="baseOffset">The offset into the ObservationWriter to start writing at.</param>
        /// <returns>The number of observations written.</returns>
        public static int WritePoses(this ObservationWriter writer, PhysicsSensorSettings settings, PoseExtractor poseExtractor, int baseOffset = 0)
        {
            var offset = baseOffset;

            if (settings.UseModelSpace)
            {
                var poses = poseExtractor.ModelSpacePoses;
                var vels  = poseExtractor.ModelSpaceVelocities;

                for (var i = 0; i < poseExtractor.NumPoses; i++)
                {
                    var pose = poses[i];
                    if (settings.UseModelSpaceTranslations)
                    {
                        writer.Add(pose.position, offset);
                        offset += 3;
                    }
                    if (settings.UseModelSpaceRotations)
                    {
                        writer.Add(pose.rotation, offset);
                        offset += 4;
                    }
                    if (settings.UseModelSpaceLinearVelocity)
                    {
                        writer.Add(vels[i], offset);
                        offset += 3;
                    }
                }
            }

            if (settings.UseLocalSpace)
            {
                var poses = poseExtractor.LocalSpacePoses;
                var vels  = poseExtractor.LocalSpaceVelocities;

                for (var i = 0; i < poseExtractor.NumPoses; i++)
                {
                    var pose = poses[i];
                    if (settings.UseLocalSpaceTranslations)
                    {
                        writer.Add(pose.position, offset);
                        offset += 3;
                    }
                    if (settings.UseLocalSpaceRotations)
                    {
                        writer.Add(pose.rotation, offset);
                        offset += 4;
                    }
                    if (settings.UseLocalSpaceLinearVelocity)
                    {
                        writer.Add(vels[i], offset);
                        offset += 3;
                    }
                }
            }

            return(offset - baseOffset);
        }