示例#1
0
        void Start()
        {
            if (this.rosbridgeIP.Equals(string.Empty))
            {
                this.rosbridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
            }
            if (this.sigverseBridgePort == 0)
            {
                this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
            }


            this.tcpClient = new System.Net.Sockets.TcpClient(this.rosbridgeIP, this.sigverseBridgePort);

            Debug.Log("Connected=" + this.tcpClient.Connected);

            this.networkStream = this.tcpClient.GetStream();

            this.networkStream.ReadTimeout  = 100000;
            this.networkStream.WriteTimeout = 100000;


            // RGB Camera
            this.xtionRGBCamera = SIGVerseUtils.FindTransformFromChild(this.transform.root, "camera_rgb_optical_frame").GetComponentInChildren <Camera>();

            int imageWidth  = this.xtionRGBCamera.targetTexture.width;
            int imageHeight = this.xtionRGBCamera.targetTexture.height;

            this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false);


            //  [camera/rgb/CameraInfo]
            string distortionModel = "plumb_bob";

            double[]         D   = { 0.0, 0.0, 0.0, 0.0, 0.0 };
            double[]         K   = { 570.3422241210938, 0.0, 319.5, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 1.0 };
            double[]         R   = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
            double[]         P   = { 570.3422241210938, 0.0, 319.5, 0.0, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
            RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false);

            this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi);

            //  [camera/rgb/Image_raw]
            string encoding    = "rgb8";
            byte   isBigendian = 0;
            uint   step        = (uint)imageWidth * 3;

            this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null);

            this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), "camera_rgb_optical_frame");


            this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
            this.imageMsg      = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge>     ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
        }
        public static List <Transform> GetLinksInChildren(Transform root)
        {
            List <Transform> linkList = new List <Transform>();

            foreach (string linkName in Enum.GetNames(typeof(Link)))
            {
                AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, linkName));
            }

            return(linkList);
        }
        private bool SetSourceTransform()
        {
            if (!this.isServer)
            {
                return(false);
            }

            //Player[] players = PhotonNetwork.PlayerListOthers;
            //int numberOfAvatars = players.Count(p => p.NickName.StartsWith("Human"));

            if (PhotonNetwork.PlayerListOthers.Count(p => p.NickName.StartsWith(this.imitatorTargetPrefix)) == 0)
            {
                Debug.LogWarning("Imitator: source not found.");
                return(false);
            }

            //this.srcAvatar = GameObject.FindGameObjectWithTag("Avatar").transform; // TODO: target should be selected in some way (e.g., a config value)
            GameObject[] avatars = GameObject.FindGameObjectsWithTag("Avatar");
            foreach (GameObject avatar in avatars)
            {
                if (avatar.name.StartsWith(this.imitatorTargetPrefix))
                {
                    this.srcAvatar = avatar.transform;
                    break;
                }
            }

            if (this.srcAvatar == null)
            {
                Debug.LogWarning("Imitator: source not found.");
                return(false);
            }

            if (this.srcAvatar.GetComponent <HumanAvatarInitializer>().IsInitialized())
            {
                List <Transform> transforms = new List <Transform>();
                foreach (Transform dsrTransform in destTransforms)
                {
                    transforms.Add(SIGVerseUtils.FindTransformFromChild(this.srcAvatar.transform, dsrTransform.name));
                }
                srcTransforms = transforms.ToArray();

                List <Transform> transformsRot = new List <Transform>();
                foreach (Transform dsrTransformRot in destTransformsRot)
                {
                    transformsRot.Add(SIGVerseUtils.FindTransformFromChild(this.srcAvatar.transform, dsrTransformRot.name));
                }
                srcTransformsRot = transformsRot.ToArray();

                return(true);
            }

            return(false);
        }
        public void SetPlaybackTargets()
        {
            this.targetTransforms = new List <Transform>();

            // Robot
            this.targetTransforms.Add(this.robot.transform);
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.base_footprint.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.arm_lift_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.arm_flex_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.arm_roll_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.wrist_flex_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.wrist_roll_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.head_pan_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.head_tilt_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.torso_lift_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_motor_dummy_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_l_proximal_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_r_proximal_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_l_distal_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(this.robot.transform, HSRCommon.Link.hand_r_distal_link.ToString()));

            // Avatar
            Transform avatar = GameObject.FindGameObjectWithTag("Avatar").transform;

            Transform[] avatarTransforms = avatar.GetComponentsInChildren <Transform>(true);

            foreach (Transform avatarTransform in avatarTransforms)
            {
                bool isTagergetTransform = true;
                foreach (string keyword in KeywordsOfAvatarPartsPathToIgnore)
                {
                    if (SIGVerse.Common.SIGVerseUtils.GetHierarchyPath(avatarTransform).Contains(keyword))
                    {
                        isTagergetTransform = false;
                    }
                }
                if (isTagergetTransform)
                {
                    this.targetTransforms.Add(avatarTransform);
                }
            }

            // Additional
            foreach (string playbackTargetTag in this.playbackTargetTags)
            {
                GameObject[] playbackTargetObjects = GameObject.FindGameObjectsWithTag(playbackTargetTag);

                foreach (GameObject playbackTargetObject in playbackTargetObjects)
                {
                    this.targetTransforms.Add(playbackTargetObject.transform);
                }
            }
        }
        public static string GetDefinitionLine(List <VideoPlayer> targetVideoPlayers)
        {
            string definitionLine = "0.0," + WorldPlaybackCommon.DataType1VideoPlayer + "," + WorldPlaybackCommon.DataType2VideoPlayerDef;             // Elapsed time is dummy.

            foreach (VideoPlayer targetVideoPlayer in targetVideoPlayers)
            {
                // Make a header line
                definitionLine += "\t" + SIGVerseUtils.GetHierarchyPath(targetVideoPlayer.transform);
            }

            return(definitionLine);
        }
        void Awake()
        {
            this.armLiftLink   = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.ArmLiftLinkName);
            this.armFlexLink   = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.ArmFlexLinkName);
            this.armRollLink   = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.ArmRollLinkName);
            this.wristFlexLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.WristFlexLinkName);
            this.wristRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.WristRollLinkName);
            this.headPanLink   = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.HeadPanLinkName);
            this.headTiltLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.HeadTiltLinkName);

            this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
        }
示例#7
0
        public static string GetDefinitionLine(List <Transform> targetTransforms)
        {
            string definitionLine = "0.0," + WorldPlaybackCommon.DataType1Transform + "," + WorldPlaybackCommon.DataType2TransformDef;             // Elapsed time is dummy.

            foreach (Transform targetTransform in targetTransforms)
            {
                // Make a header line
                definitionLine += "\t" + SIGVerseUtils.GetHierarchyPath(targetTransform);
            }

            return(definitionLine);
        }
示例#8
0
        public static List <Transform> GetLinksInChildren(Transform root)
        {
            List <Transform> linkList = new List <Transform>();

            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseFootPrintName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, ArmLiftLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, ArmFlexLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, ArmRollLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, WristFlexLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, WristRollLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandPalmLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandCameraFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandCameraRgbFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLProximalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLSpringProximalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLMimicDistalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLDistalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLFingerTipFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandLFingerVacuumFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandMotorDummyLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRProximalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRSpringProximalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRMimicDistalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRDistalLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HandRFingerTipFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, WristFtSensorFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseImuFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRangeSensorLink));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRollLlinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLDriveWheelLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLPassiveWheelXFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLPassiveWheelYFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseLPassiveWheelZLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRDriveWheelLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRPassiveWheelXFramName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRPassiveWheelYFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, BaseRPassiveWheelZLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, TorsoLiftLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadPanLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadTiltLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadCenterCameraFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadCenterCameraRgbFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadLStereoCameraLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadLStereoCameraRgbFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRStereoCameraLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRStereoCameraRgbFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRgbdSensorLinkName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRgbdSensorRgbFrameName));
            AddLink(linkList, SIGVerseUtils.FindTransformFromChild(root, HeadRgbdSensorDepthFrameName));

            return(linkList);
        }
        protected override void Awake()
        {
            if (CleanupConfig.Instance.configFileInfo.playbackType == WorldPlaybackCommon.PlaybackTypePlay)
            {
                // Activate all grasping candidates
                GameObject graspingCandidatesObj = GameObject.Find("GraspingCandidates");

                foreach (Transform graspingCandidate in graspingCandidatesObj.transform)
                {
                    graspingCandidate.gameObject.SetActive(true);

                    graspingCandidate.position = new Vector3(0.0f, -5.0f, 0.0f);                     // Wait in the ground

                    // Disable rigidbodies
                    Rigidbody[] rigidbodies = graspingCandidate.GetComponentsInChildren <Rigidbody>(true);
                    foreach (Rigidbody rigidbody in rigidbodies)
                    {
                        rigidbody.isKinematic = true;
                    }

                    // Disable colliders
                    Collider[] colliders = graspingCandidate.GetComponentsInChildren <Collider>(true);
                    foreach (Collider collider in colliders)
                    {
                        collider.enabled = false;
                    }
                }
            }

            base.Awake();

            // Robot
            Transform robot = GameObject.FindGameObjectWithTag("Robot").transform;

            this.targetTransforms.Add(robot);

            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.base_footprint.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.arm_lift_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.arm_flex_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.arm_roll_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.wrist_flex_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.wrist_roll_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.head_pan_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.head_tilt_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.torso_lift_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_motor_dummy_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_l_proximal_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_r_proximal_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_l_distal_link.ToString()));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.Link.hand_r_distal_link.ToString()));
        }
        public PlaybackVideoPlayerEventController(WorldPlaybackCommon playbackCommon)
        {
            this.common = playbackCommon;

            // Video Player
            this.targetVideoPlayers = this.common.GetTargetVideoPlayers();

            this.targetVideoPlayerPathMap = new Dictionary <string, VideoPlayer>();

            foreach (VideoPlayer targetVideoPlayer in this.targetVideoPlayers)
            {
                this.targetVideoPlayerPathMap.Add(SIGVerseUtils.GetHierarchyPath(targetVideoPlayer.transform), targetVideoPlayer);
            }
        }
示例#11
0
        void Awake()
        {
            this.trajectoryInfoMap = new Dictionary <string, TrajectoryInfo>();
            this.trajectoryInfoMap.Add(HSRCommon.OmniOdomX_JointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.OmniOdomY_JointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.OmniOdomT_JointName, null);

            this.baseFootprintPosNoise  = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintPosNoiseName);
            this.baseFootprintRotNoise  = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRotNoiseName);
            this.baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);

            this.initialPosition = this.baseFootprintRigidbody.position;//save initial position.
            this.initialRotation = this.baseFootprintRigidbody.rotation;
        }//Awake
        void Awake()
        {
            this.armLiftLink       = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_lift_link.ToString());
            this.armFlexLink       = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_flex_link.ToString());
            this.armRollLink       = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_roll_link.ToString());
            this.wristFlexLink     = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.wrist_flex_link.ToString());
            this.wristRollLink     = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.wrist_roll_link.ToString());
            this.headPanLink       = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_pan_link.ToString());
            this.headTiltLink      = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_tilt_link.ToString());
            this.handLProximalLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.hand_l_proximal_link.ToString());
            this.handRProximalLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.hand_r_proximal_link.ToString());

            this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
        }
示例#13
0
        protected void Awake()
        {
            this.collisionEffect = (GameObject)Resources.Load(CompetitionUtils.CollisionEffectPath);

            GameObject robot = GameObject.FindGameObjectWithTag(TagRobot);

            this.hsrBaseColliders = new List <Collider>();
            this.hsrBaseColliders.AddRange(SIGVerseUtils.FindTransformFromChild(robot.transform.root, HSRCommon.BaseName).GetComponentsInChildren <Collider>());
            this.hsrBaseColliders.AddRange(SIGVerseUtils.FindTransformFromChild(robot.transform.root, HSRCommon.BumperName).GetComponentsInChildren <Collider>());

            this.hsrHandColliders = new List <Collider>();
            this.hsrHandColliders.AddRange(SIGVerseUtils.FindTransformFromChild(robot.transform.root, HSRCommon.WristRollLinkName).GetComponentsInChildren <Collider>());

            this.hsrGraspingDetector = robot.GetComponent <HSRGraspingDetector>();
        }
        protected static string GetDataLine(string elapsedTime, Collision collision, float collisionVelocity, float effectScale, string dataType)
        {
            Vector3 contactAve = SIGVerseUtils.CalcContactAveragePoint(collision);

            string dataLine = elapsedTime + "," + dataType;

            dataLine += "\t" + contactAve.x + "\t" + contactAve.y + "\t" + contactAve.z + "\t" + effectScale;

            // Following data is unused now
            dataLine += "\t" + collisionVelocity +
                        "\t" + SIGVerseUtils.GetHierarchyPath(collision.contacts[0].thisCollider.transform) +
                        "\t" + SIGVerseUtils.GetHierarchyPath(collision.contacts[0].otherCollider.transform);

            return(dataLine);
        }
        public IEnumerator DisplayEndMessage()
        {
            yield return(new WaitForSecondsRealtime(7));

            string endMessage = "All sessions have ended";

            SIGVerseLogger.Info(endMessage);
            this.tool.AddSpeechQueModerator(endMessage);

            string panelMessage = endMessage + "\n"
                                  + "(" + SIGVerseUtils.GetOrdinal(HandymanConfig.Instance.numberOfTrials) + ": " + this.lastPanelMessage.Replace("\n", " - ") + ")";

            this.SendPanelNotice(panelMessage, 80, PanelNoticeStatus.Blue, false);

            this.bgmAudioSource.enabled = false;
        }
        void Awake()
        {
            this.baseFootprint          = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.Link.base_footprint.ToString());
            this.baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);
            this.baseFootprintPosNoise  = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintPosNoiseName);
            this.baseFootprintRotNoise  = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRotNoiseName);

            this.twist = this.transform.GetComponentInChildren <HSRSubTwist>(true);

            this.trajectoryInfoMap = new Dictionary <HSRCommon.Joint, TrajectoryData>()
            {
                { HSRCommon.Joint.odom_x, null },
                { HSRCommon.Joint.odom_y, null },
                { HSRCommon.Joint.odom_t, null },
            };
        }
示例#17
0
        //対象物体のパスとトランスフォームのマップを作成
        private void SetTargetObjectPathTranformMap()
        {
            this.targetPathTransformMap.Clear();

            //アバタの各部位
            foreach (Transform transform in this.instructor.GetComponentsInChildren <Transform>())
            {
                this.targetPathTransformMap.Add(SIGVerseUtils.GetHierarchyPath(transform), transform);
            }

            //対象物体
            foreach (GameObject targetObj in this.playbackCommon.GetTargetObjects())
            {
                this.targetPathTransformMap.Add(SIGVerseUtils.GetHierarchyPath(targetObj.transform), targetObj.transform);
            }
        }
示例#18
0
        void Awake()
        {
            this.torsoLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.torso_lift_link.ToString());
            this.headPanLink   = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_pan_link.ToString());
            this.headTiltLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_tilt_link.ToString());

            this.lShoulderPanLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_shoulder_pan_link.ToString());
            this.lShoulderLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_shoulder_lift_link.ToString());
            this.lUpperArmRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_upper_arm_roll_link.ToString());
            this.lElbowFlexLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_elbow_flex_link.ToString());
            this.lForearmRollLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_forearm_roll_link.ToString());
            this.lWristFlexLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_wrist_flex_link.ToString());
            this.lWristRollLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_wrist_roll_link.ToString());

            this.rShoulderPanLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_shoulder_pan_link.ToString());
            this.rShoulderLiftLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_shoulder_lift_link.ToString());
            this.rUpperArmRollLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_upper_arm_roll_link.ToString());
            this.rElbowFlexLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_elbow_flex_link.ToString());
            this.rForearmRollLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_forearm_roll_link.ToString());
            this.rWristFlexLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_wrist_flex_link.ToString());
            this.rWristRollLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_wrist_roll_link.ToString());

            this.torsoLiftLinkIniPosZ = this.torsoLiftLink.localPosition.z;

            this.trajectoryInfoMap = new Dictionary <PR2Common.Joint, TrajectoryInfo>()
            {
                { PR2Common.Joint.torso_lift_joint, null },
                { PR2Common.Joint.head_pan_joint, null },
                { PR2Common.Joint.head_tilt_joint, null },
                { PR2Common.Joint.l_shoulder_pan_joint, null },
                { PR2Common.Joint.l_shoulder_lift_joint, null },
                { PR2Common.Joint.l_upper_arm_roll_joint, null },
                { PR2Common.Joint.l_elbow_flex_joint, null },
                { PR2Common.Joint.l_forearm_roll_joint, null },
                { PR2Common.Joint.l_wrist_flex_joint, null },
                { PR2Common.Joint.l_wrist_roll_joint, null },
                { PR2Common.Joint.r_shoulder_pan_joint, null },
                { PR2Common.Joint.r_shoulder_lift_joint, null },
                { PR2Common.Joint.r_upper_arm_roll_joint, null },
                { PR2Common.Joint.r_elbow_flex_joint, null },
                { PR2Common.Joint.r_forearm_roll_joint, null },
                { PR2Common.Joint.r_wrist_flex_joint, null },
                { PR2Common.Joint.r_wrist_roll_joint, null }
            };

            this.trajectoryKeyList = new List <PR2Common.Joint>(trajectoryInfoMap.Keys);
        }
示例#19
0
        public PlaybackTransformEventController(WorldPlaybackCommon playbackCommon)
        {
            this.common = playbackCommon;

            // Transform
            this.targetTransforms = this.common.GetTargetTransforms();

            this.targetTransformPathMap = new Dictionary <string, Transform>();

            foreach (Transform targetTransform in this.targetTransforms)
            {
                this.targetTransformPathMap.Add(SIGVerseUtils.GetHierarchyPath(targetTransform), targetTransform);
            }

            this.IsRigidbodiesDisable = true;
            this.IsCollidersDisable   = true;
        }
 void Awake()
 {
     if (this.handType == HandType.Left)
     {
         this.gripperLFingerLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_l_finger_link.ToString());
         this.gripperRFingerLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_r_finger_link.ToString());
         this.gripperLFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_l_finger_tip_link.ToString());
         this.gripperRFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.l_gripper_r_finger_tip_link.ToString());
     }
     else
     {
         this.gripperLFingerLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_l_finger_link.ToString());
         this.gripperRFingerLink    = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_r_finger_link.ToString());
         this.gripperLFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_l_finger_tip_link.ToString());
         this.gripperRFingerTipLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.r_gripper_r_finger_tip_link.ToString());
     }
 }
示例#21
0
        private void PreProcess()
        {
            this.sessionManager.ChangeEnvironment(this.numberOfSession);
            this.ResetAvatarTransform();

            this.sessionManager.ActivateRobot();

            if (!this.isPracticeMode)
            {
                this.rosConnections = SIGVerseUtils.FindObjectsOfInterface <IRosConnection>();
                SIGVerseLogger.Info("ROS connection : count=" + this.rosConnections.Length);
            }
            else
            {
                SIGVerseLogger.Info("No ROS connection (Practice mode)");
            }

            //this.currentTaskInfo = this.sessionManager.GetCurrentTaskInfo();
            this.currentTaskInfo = this.sessionManager.GetTaskInfo(this.numberOfSession);

            this.taskInfoForRobot = new SIGVerse.RosBridge.human_navigation.HumanNaviTaskInfo();
            string environmentName = this.sessionManager.GetEnvironment().name;

            this.taskInfoForRobot.environment_id = environmentName.Substring(0, environmentName.Length - 3);
            this.SetObjectListToHumanNaviTaskInfo();
            this.SetFurnitureToHumanNaviTaskInfo();
            this.SetDestinationToHumanNaviTaskInfo();

            this.waitingTime = 0.0f;

            this.interruptedReason = string.Empty;

            this.objectIdInLeftHand               = "";
            this.objectIdInRightHand              = "";
            this.objectIdInLeftHandPreviousFrame  = "";             // Tentative Code
            this.objectIdInRightHandPreviousFrame = "";             // Tentative Code

            this.alreadyGraspedObjects = new List <string>();

            this.InitializePlayback();

            //this.countWrongObjectsGrasp = 0;

//			SIGVerseLogger.Info("End of PreProcess");
        }
示例#22
0
        //録画を行う対象物体を格納
        private void SetTargetTransforms()
        {
            this.targetTransformInstances.Clear();

            //アバタの各部位
            foreach (Transform avatarTransform in this.playbackCommon.GetAvatar().GetComponentsInChildren <Transform>())
            {
                if (!this.ignoringKeywords.Exists(x => SIGVerseUtils.GetHierarchyPath(avatarTransform).Contains(x)))
                {
                    this.targetTransformInstances.Add(avatarTransform);
                }
            }

            //対象物体
            foreach (GameObject graspingCandidate in this.playbackCommon.GetTargetObjects())
            {
                this.targetTransformInstances.Add(graspingCandidate.transform);
            }
        }
        void Awake()
        {
            this.torsoLiftLink          = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.torso_lift_link.ToString());
            this.head1Link              = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_1_link.ToString());
            this.head2Link              = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_2_link.ToString());
            this.arm1Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_1_link.ToString());
            this.arm2Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_2_link.ToString());
            this.arm3Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_3_link.ToString());
            this.arm4Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_4_link.ToString());
            this.arm5Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_5_link.ToString());
            this.arm6Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_6_link.ToString());
            this.arm7Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_7_link.ToString());
            this.gripperLeftFingerLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_left_finger_link.ToString());
            this.gripperRightFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_right_finger_link.ToString());

            this.torsoLiftLinkIniPosZ          = this.torsoLiftLink.localPosition.z;
            this.arm1LinkIniRotZ               = this.arm1Link.localEulerAngles.z;
            this.arm3LinkIniRotZ               = this.arm3Link.localEulerAngles.z;
            this.arm4LinkIniRotY               = this.arm4Link.localEulerAngles.y;
            this.arm6LinkIniRotY               = this.arm6Link.localEulerAngles.y;
            this.arm7LinkIniRotZ               = this.arm7Link.localEulerAngles.z;
            this.gripperLeftFingerLinkIniPosX  = this.gripperLeftFingerLink.localPosition.x;
            this.gripperRightFingerLinkIniPosX = this.gripperRightFingerLink.localPosition.x;

            this.trajectoryInfoMap = new Dictionary <TIAGoCommon.Joint, TrajectoryInfo>()
            {
                { TIAGoCommon.Joint.torso_lift_joint, null },
                { TIAGoCommon.Joint.head_1_joint, null },
                { TIAGoCommon.Joint.head_2_joint, null },
                { TIAGoCommon.Joint.arm_1_joint, null },
                { TIAGoCommon.Joint.arm_2_joint, null },
                { TIAGoCommon.Joint.arm_3_joint, null },
                { TIAGoCommon.Joint.arm_4_joint, null },
                { TIAGoCommon.Joint.arm_5_joint, null },
                { TIAGoCommon.Joint.arm_6_joint, null },
                { TIAGoCommon.Joint.arm_7_joint, null },
                { TIAGoCommon.Joint.gripper_left_finger_joint, null },
                { TIAGoCommon.Joint.gripper_right_finger_joint, null },
            };

            this.trajectoryKeyList = new List <TIAGoCommon.Joint>(trajectoryInfoMap.Keys);
        }
示例#24
0
        public void scoreCollision(GameObject obj, Collision collision)
        {
            if (recordScore && collision.relativeVelocity.magnitude > scoreThreshold)
            {
                //Arm-Obstacle
                String colPathArm = SIGVerseUtils.GetHierarchyPath(collision.collider.transform);
                bool   colArm     = colPathArm.Contains("arm_lift_link");
                if (colArm)
                {
                    //SIGVerseLogger.Info ("************************************************************************************************ ARM COLLISION  ************"  );
                    collisionA++;
                }
                //Obstacle-Obstacle
                bool colObs = collision.gameObject.CompareTag(TagObstacleCandidates);
                if (colObs)
                {
                    //SIGVerseLogger.Info ("************************************************************************************************ OBSTACLE COLLISION  ************"  );
                    collisionO++;
                }

                //Target-Obstacle
                bool colTar = collision.gameObject.CompareTag(TagGraspingCandidates);
                if (colTar)
                {
                    //SIGVerseLogger.Info ("************************************************************************************************ TARGET COLLISION  ************"  );
                    collisionT++;
                }
                //Furniture-Obstacle
                String colPathFur = SIGVerseUtils.GetHierarchyPath(collision.collider.transform);
                bool   colFur     = colPathFur.Contains("Tables");
                if (colFur)
                {
                    //SIGVerseLogger.Info ("************************************************************************************************ FURNITURE COLLISION  ************"  );
                    collisionF++;
                }

                if (collision.relativeVelocity.magnitude > maxVel)
                {
                    maxVel = collision.relativeVelocity.magnitude;
                }
            }
        }
示例#25
0
        void Awake()
        {
            try
            {
                // Playback system
                this.playbackRecorder = this.playbackManager.GetComponent <HumanNaviPlaybackRecorder>();

                // Avatar
                this.initialAvatarPosition = this.avatar.transform.position;
                this.initialAvatarRotation = this.avatar.transform.eulerAngles;

                // GUI
                this.mainMenu            = GameObject.FindGameObjectWithTag("MainMenu");
                this.panelMainController = mainMenu.GetComponent <PanelMainController>();

                this.noticePanelForAvatar.SetActive(false);
                this.noticeTextForAvatar.text = "";

                // MessageMap
                this.receivedMessageMap = new Dictionary <string, bool>();
                this.receivedMessageMap.Add(MsgIamReady, false);
                this.receivedMessageMap.Add(MsgGetAvatarPose, false);
                this.receivedMessageMap.Add(MsgConfirmSpeechState, false);
                this.receivedMessageMap.Add(MsgGiveUp, false);

                // ROSBridge
                // (Should be read after the robot is instantiated (after Awake process of SessionManager))
                this.rosConnections = SIGVerseUtils.FindObjectsOfInterface <IRosConnection>();
                SIGVerseLogger.Info("ROS connection : count=" + this.rosConnections.Length);

                // Timer
                this.stepTimer = new StepTimer();
            }
            catch (Exception exception)
            {
                Debug.LogError(exception);
                SIGVerseLogger.Error(exception.Message);
                SIGVerseLogger.Error(exception.StackTrace);
                this.ApplicationQuitAfter1sec();
            }
        }
        protected override void Awake()
        {
            base.Awake();

            // Robot
            Transform robot = GameObject.FindGameObjectWithTag("Robot").transform;

            this.targetTransforms.Add(robot);

            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.BaseFootPrintName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.ArmLiftLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.ArmFlexLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.ArmRollLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.WristFlexLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.WristRollLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HeadPanLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HeadTiltLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.TorsoLiftLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HandLProximalLinkName));
            this.targetTransforms.Add(SIGVerseUtils.FindTransformFromChild(robot, HSRCommon.HandRProximalLinkName));
        }
        private void Awake()
        {
            try
            {
                // For practice mode
                if (HumanNaviConfig.Instance.configInfo.executionMode == (int)ExecutionMode.Practice)
                {
                    this.GetComponent <HumanNaviPubTaskInfo>().enabled     = false;
                    this.GetComponent <HumanNaviPubMessage>().enabled      = false;
                    this.GetComponent <HumanNaviSubMessage>().enabled      = false;
                    this.GetComponent <HumanNaviPubAvatarStatus>().enabled = false;
                    this.GetComponent <HumanNaviPubObjectStatus>().enabled = false;

                    robot.transform.Find("RosBridgeScripts").gameObject.SetActive(false);
                    robot.GetComponentInChildren <HumanNaviSubGuidanceMessage>().enabled = false;

                    foreach (Camera camera in robot.transform.GetComponentsInChildren <Camera>())
                    {
                        camera.gameObject.SetActive(false);
                    }
                }

                this.robot.SetActive(false);
                this.robotBaseFootprint = SIGVerseUtils.FindTransformFromChild(this.robot.transform, "base_footprint");

                this.InitializeEnvironments();

                this.tts = this.robot.transform.Find("CompetitionScripts").GetComponent <SAPIVoiceSynthesisExternal>();

                this.InitializeTaskInfo();
            }
            catch (Exception e)
            {
                SIGVerseLogger.Error(e.Message);
                this.ApplicationQuitAfter1sec();
            }
        }
        void Awake()
        {
            List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);

            foreach (UnityEngine.Transform localLink in localLinkList)
            {
                if (localLink.name == HSRCommon.Link.base_footprint.ToString())
                {
                    TransformStamped localTransformStamped = new TransformStamped();
                    localTransformStamped.header.frame_id = HSRCommon.OdomName;
                    localTransformStamped.child_frame_id  = localLink.name;

                    UnityEngine.Transform baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);

                    TfInfo localTfInfo = new TfInfo(baseFootprintRigidbody, localTransformStamped);

                    this.localTfInfoList.Add(localTfInfo);
                }
                else
                {
                    TransformStamped localTransformStamped = new TransformStamped();
                    localTransformStamped.header.frame_id = localLink.parent.name;
                    localTransformStamped.child_frame_id  = localLink.name;

                    TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);

                    this.localTfInfoList.Add(localTfInfo);
                }
            }

            this.synchronizer = this.GetComponent <HSRPubSynchronizer>();

            this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();

            this.isUsingThread = this.synchronizer.useThread;
        }
        void Awake()
        {
            this.torsoLiftLink          = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.torso_lift_link.ToString());
            this.head1Link              = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_1_link.ToString());
            this.head2Link              = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.head_2_link.ToString());
            this.arm1Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_1_link.ToString());
            this.arm2Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_2_link.ToString());
            this.arm3Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_3_link.ToString());
            this.arm4Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_4_link.ToString());
            this.arm5Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_5_link.ToString());
            this.arm6Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_6_link.ToString());
            this.arm7Link               = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.arm_7_link.ToString());
            this.gripperLeftFingerLink  = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_left_finger_link.ToString());
            this.gripperRightFingerLink = SIGVerseUtils.FindTransformFromChild(this.transform.root, Link.gripper_right_finger_link.ToString());

            this.torsoLiftLinkIniPosZ          = this.torsoLiftLink.localPosition.z;
            this.arm1LinkIniRotZ               = this.arm1Link.localEulerAngles.z;
            this.arm3LinkIniRotZ               = this.arm3Link.localEulerAngles.z;
            this.arm4LinkIniRotY               = this.arm4Link.localEulerAngles.y;
            this.arm6LinkIniRotY               = this.arm6Link.localEulerAngles.y;
            this.arm7LinkIniRotZ               = this.arm7Link.localEulerAngles.z;
            this.gripperLeftFingerLinkIniPosX  = this.gripperLeftFingerLink.localPosition.x;
            this.gripperRightFingerLinkIniPosX = this.gripperRightFingerLink.localPosition.x;
        }
示例#30
0
        public PlaybackTransformEventController(string filePath)
        {
            this.targetTransforms = new List <Transform>();

            foreach (GameObject rootObj in SceneManager.GetActiveScene().GetRootGameObjects())
            {
                if (!rootObj.activeInHierarchy)
                {
                    continue;
                }

                this.targetTransforms.AddRange(rootObj.GetComponentsInChildren <Transform>(true));
            }

            this.targetTransformPathMap = new Dictionary <string, Transform>();

            foreach (Transform targetTransform in this.targetTransforms)
            {
                this.targetTransformPathMap.Add(SIGVerseUtils.GetHierarchyPath(targetTransform), targetTransform);
            }

            this.IsRigidbodiesDisable = true;
            this.IsCollidersDisable   = true;
        }