private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
        {
            for (int i = 0; i < msg.joint_names.Count; i++)
            {
                HSRCommon.Joint joint = (HSRCommon.Joint)Enum.Parse(typeof(HSRCommon.Joint), msg.joint_names[i]);

                List <float> positions = new List <float>();
                List <float> durations = new List <float>();

                for (int pointIndex = 0; pointIndex < msg.points.Count; pointIndex++)
                {
                    positions.Add(HSRCommon.GetClampedPosition((float)msg.points[pointIndex].positions[i], joint));
                    durations.Add((float)msg.points[pointIndex].time_from_start.secs + (float)msg.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
                }

                switch (joint)
                {
                case HSRCommon.Joint.arm_lift_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, +this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ); break; }

                case HSRCommon.Joint.arm_flex_joint:  { this.SetJointTrajectoryRotation(joint, durations, positions, +this.armFlexLink.localEulerAngles.y); break; }

                case HSRCommon.Joint.arm_roll_joint:  { this.SetJointTrajectoryRotation(joint, durations, positions, -this.armRollLink.localEulerAngles.z); break; }

                case HSRCommon.Joint.wrist_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.wristFlexLink.localEulerAngles.y); break; }

                case HSRCommon.Joint.wrist_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.wristRollLink.localEulerAngles.z); break; }

                case HSRCommon.Joint.head_pan_joint:  { this.SetJointTrajectoryRotation(joint, durations, positions, -this.headPanLink.localEulerAngles.z); break; }

                case HSRCommon.Joint.head_tilt_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.headTiltLink.localEulerAngles.y); break; }

                case HSRCommon.Joint.hand_motor_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.handMotorDummyLink.localEulerAngles.x); break; }
                }
            }
        }
        private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
        {
            for (int i = 0; i < msg.joint_names.Count; i++)
            {
                HSRCommon.Joint joint = (HSRCommon.Joint)Enum.Parse(typeof(HSRCommon.Joint), msg.joint_names[i]);

                List <float> positions = new List <float>();
                List <float> durations = new List <float>();

                for (int pointIndex = 0; pointIndex < msg.points.Count; pointIndex++)
                {
                    positions.Add(HSRCommon.GetClampedPosition((float)msg.points[pointIndex].positions[i], joint));
                    durations.Add((float)msg.points[pointIndex].time_from_start.secs + (float)msg.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
                }

                switch (joint)
                {
                case HSRCommon.Joint.odom_x:
                case HSRCommon.Joint.odom_y:
                case HSRCommon.Joint.odom_t: { this.trajectoryInfoMap[joint] = new TrajectoryData(Time.time, durations, positions); break; }

                default: { throw new Exception("NOT odom joint"); }
                }
            }
        }
        protected override void Update()
        {
            base.Update();

            this.elapsedTime += UnityEngine.Time.deltaTime;

            if (this.elapsedTime < this.sendingInterval * 0.001)
            {
                return;
            }

            this.elapsedTime = 0.0f;

            List <double> positions = new List <double>()
            {
                { this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ },                                                                                //1 arm_lift_joint
                { HSRCommon.GetNormalizedJointEulerAngle(+this.armFlexLink.localEulerAngles.y, HSRCommon.Joint.arm_flex_joint) * Mathf.Deg2Rad },              // 2
                { HSRCommon.GetNormalizedJointEulerAngle(-this.armRollLink.localEulerAngles.z, HSRCommon.Joint.arm_roll_joint) * Mathf.Deg2Rad },              // 3
                { HSRCommon.GetNormalizedJointEulerAngle(+this.wristFlexLink.localEulerAngles.y, HSRCommon.Joint.wrist_flex_joint) * Mathf.Deg2Rad },          // 4
                { HSRCommon.GetNormalizedJointEulerAngle(-this.wristRollLink.localEulerAngles.z, HSRCommon.Joint.wrist_roll_joint) * Mathf.Deg2Rad },          // 5
                { HSRCommon.GetNormalizedJointEulerAngle(-this.headPanLink.localEulerAngles.z, HSRCommon.Joint.head_pan_joint) * Mathf.Deg2Rad },              // 6
                { HSRCommon.GetNormalizedJointEulerAngle(+this.headTiltLink.localEulerAngles.y, HSRCommon.Joint.head_tilt_joint) * Mathf.Deg2Rad },            // 7
                { HSRCommon.GetNormalizedJointEulerAngle(+this.handLProximalLink.localEulerAngles.x, HSRCommon.Joint.hand_l_proximal_joint) * Mathf.Deg2Rad }, // 8 hand_l_spring_proximal_joint
                { HSRCommon.GetNormalizedJointEulerAngle(+this.handRProximalLink.localEulerAngles.x, HSRCommon.Joint.hand_r_proximal_joint) * Mathf.Deg2Rad }, // 9 hand_r_spring_proximal_joint
            };

            this.jointState.header.Update();
            this.jointState.position = positions;

            this.publisher.Publish(this.jointState);
        }
        void Awake()
        {
            this.armLiftLink       = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmLiftLinkName);
            this.armFlexLink       = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmFlexLinkName);
            this.armRollLink       = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmRollLinkName);
            this.wristFlexLink     = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristFlexLinkName);
            this.wristRollLink     = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristRollLinkName);
            this.headPanLink       = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadPanLinkName);
            this.headTiltLink      = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadTiltLinkName);
            this.torsoLiftLink     = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.TorsoLiftLinkName);
            this.handLProximalLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HandLProximalLinkName);
            this.handRProximalLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HandRProximalLinkName);

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

            this.trajectoryInfoMap = new Dictionary <string, TrajectoryInfo>();
            this.trajectoryInfoMap.Add(HSRCommon.ArmLiftJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.ArmFlexJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.ArmRollJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.WristFlexJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.WristRollJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.HeadPanJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.HeadTiltJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.HandLProximalJointName, null);
            this.trajectoryInfoMap.Add(HSRCommon.HandRProximalJointName, null);
        }
        protected void FixedUpdate()
        {
            foreach (HSRCommon.Joint joint in this.trajectoryKeyList)
            {
                if (this.trajectoryInfoMap[joint] == null)
                {
                    continue;
                }

                switch (joint)
                {
                case HSRCommon.Joint.arm_lift_joint:
                {
                    float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint);

                    this.armLiftLink.localPosition   = new Vector3(this.armLiftLink.localPosition.x, this.armLiftLink.localPosition.y, this.armLiftLinkIniPosZ + newPos);
                    this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos / 2.0f);
                    break;
                }

                case HSRCommon.Joint.arm_flex_joint:   { this.UpdateLinkAngle(this.armFlexLink, joint, Vector3.up);   break; }

                case HSRCommon.Joint.arm_roll_joint:   { this.UpdateLinkAngle(this.armRollLink, joint, Vector3.back); break; }

                case HSRCommon.Joint.wrist_flex_joint: { this.UpdateLinkAngle(this.wristFlexLink, joint, Vector3.up);   break; }

                case HSRCommon.Joint.wrist_roll_joint: { this.UpdateLinkAngle(this.wristRollLink, joint, Vector3.back); break; }

                case HSRCommon.Joint.head_pan_joint:   { this.UpdateLinkAngle(this.headPanLink, joint, Vector3.back); break; }

                case HSRCommon.Joint.head_tilt_joint:  { this.UpdateLinkAngle(this.headTiltLink, joint, Vector3.up);   break; }

                case HSRCommon.Joint.hand_motor_joint:
                {
                    float newPos = HSRCommon.GetNormalizedJointEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg, joint);

                    // Grasping and hand closing
                    if (this.graspedObject != null && this.IsAngleDecreasing(newPos, this.handMotorDummyLink.localEulerAngles.x))
                    {
                        // Have to stop
                        this.trajectoryInfoMap[joint] = null;
                    }
                    // Otherwise
                    else
                    {
                        this.handMotorDummyLink.localEulerAngles = new Vector3(+newPos, this.handMotorDummyLink.localEulerAngles.y, this.handMotorDummyLink.localEulerAngles.z);
                        this.handLProximalLink.localEulerAngles  = new Vector3(+newPos, this.handLProximalLink.localEulerAngles.y, this.handLProximalLink.localEulerAngles.z);
                        this.handRProximalLink.localEulerAngles  = new Vector3(-newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
                        this.handLDistalLink.localEulerAngles    = new Vector3(-newPos, this.handLDistalLink.localEulerAngles.y, this.handLDistalLink.localEulerAngles.z);
                        this.handRDistalLink.localEulerAngles    = new Vector3(+newPos, this.handRDistalLink.localEulerAngles.y, this.handRDistalLink.localEulerAngles.z);
                    }
                    break;
                }
                }
            }
        }
Example #6
0
        void Awake()
        {
            this.armLiftLink   = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmLiftLinkName);
            this.armFlexLink   = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmFlexLinkName);
            this.armRollLink   = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmRollLinkName);
            this.wristFlexLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristFlexLinkName);
            this.wristRollLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristRollLinkName);
            this.headPanLink   = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadPanLinkName);
            this.headTiltLink  = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadTiltLinkName);

            this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
        }
        private static float GetPositionAndUpdateTrajectory(Dictionary <HSRCommon.Joint, TrajectoryInfo> trajectoryInfoMap, HSRCommon.Joint joint)
        {
            float minSpeed = HSRCommon.GetMinJointSpeed(joint);
            float maxSpeed = HSRCommon.GetMaxJointSpeed(joint);

            TrajectoryInfo trajectoryInfo = trajectoryInfoMap[joint];

            int targetPointIndex = GetTargetPointIndex(trajectoryInfo);

            float speed = 0.0f;

            if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Durations[targetPointIndex])
            {
                speed = maxSpeed;
            }
            else
            {
                speed = Mathf.Abs((trajectoryInfo.GoalPositions[targetPointIndex] - trajectoryInfo.CurrentPosition) / (trajectoryInfo.Durations[targetPointIndex] - (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime)));
                speed = Mathf.Clamp(speed, minSpeed, maxSpeed);
            }

            // Calculate position
            float newPosition;
            float movingDistance = speed * (Time.time - trajectoryInfo.CurrentTime);

            if (movingDistance > Mathf.Abs(trajectoryInfo.GoalPositions[targetPointIndex] - trajectoryInfo.CurrentPosition))
            {
                newPosition = trajectoryInfo.GoalPositions[targetPointIndex];
                trajectoryInfoMap[joint] = null;
            }
            else
            {
                trajectoryInfo.CurrentTime = Time.time;

                if (trajectoryInfo.GoalPositions[targetPointIndex] > trajectoryInfo.CurrentPosition)
                {
                    trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance;
                    newPosition = trajectoryInfo.CurrentPosition;
                }
                else
                {
                    trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance;
                    newPosition = trajectoryInfo.CurrentPosition;
                }
            }

            return(newPosition);
        }
        private void UpdateLinkAngle(Transform link, HSRCommon.Joint joint, Vector3 axis)
        {
            float newPos = HSRCommon.GetNormalizedJointEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg, joint);

            if (Mathf.Abs(axis.x) == 1)
            {
                link.localEulerAngles = new Vector3(0.0f, link.localEulerAngles.y, link.localEulerAngles.z) + newPos * axis;
            }
            else if (Mathf.Abs(axis.y) == 1)
            {
                link.localEulerAngles = new Vector3(link.localEulerAngles.x, 0.0f, link.localEulerAngles.z) + newPos * axis;
            }
            else if (Mathf.Abs(axis.z) == 1)
            {
                link.localEulerAngles = new Vector3(link.localEulerAngles.x, link.localEulerAngles.y, 0.0f) + newPos * axis;
            }
        }
Example #9
0
        void Awake()
        {
            List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);

            foreach (UnityEngine.Transform localLink in localLinkList)
            {
                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.publishSequenceNumber = HSRPubSynchronizer.GetAssignedSequenceNumber();
        }
Example #10
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 = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);

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

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

            this.transformStampedMsg = new SIGVerseROSBridgeMessage <TransformStamped[]>("publish", this.topicName, "sigverse/TfList", null);
        }
Example #11
0
        protected override void Update()
        {
            base.Update();

            this.elapsedTime += UnityEngine.Time.deltaTime;

            if (this.elapsedTime < this.sendingInterval * 0.001)
            {
                return;
            }

            this.elapsedTime = 0.0f;

            List <double> positions = new List <double>();

            //1 ArmLiftJoint
            positions.Add(this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
            //2 ArmFlexJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, HSRCommon.ArmFlexJointName) * Mathf.Deg2Rad);
            //3 ArmRollJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, HSRCommon.ArmRollJointName) * Mathf.Deg2Rad);
            //4 WristFlexJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, HSRCommon.WristFlexJointName) * Mathf.Deg2Rad);
            //5 WristRollJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, HSRCommon.WristRollJointName) * Mathf.Deg2Rad);
            //6 HeadPanJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, HSRCommon.HeadPanJointName) * Mathf.Deg2Rad);
            //7 HeadTiltJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, HSRCommon.HeadTiltJointName) * Mathf.Deg2Rad);
            //8 HandLSpringProximalJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, HSRCommon.HandLProximalJointName) * Mathf.Deg2Rad);
            //9 HandRSpringProximalJoint
            positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, HSRCommon.HandRProximalJointName) * Mathf.Deg2Rad);

            this.jointState.header.Update();
            this.jointState.position = positions;

//			float position = HSRCommon.GetClampedPosition(value, name);

            this.publisher.Publish(this.jointState);
        }
        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 = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);

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

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

            this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.sensorLink.name);

            this.laserScan = new LaserScanForSIGVerseBridge();

            this.numLines = (int)Mathf.Round(LaserAngle / LaserAngularResolution) + 1;

            this.laserScan.header = this.header;

            this.laserScan.angle_min       = -HalfOfLaserAngle * Mathf.Deg2Rad;
            this.laserScan.angle_max       = +HalfOfLaserAngle * Mathf.Deg2Rad;
            this.laserScan.angle_increment = LaserAngularResolution * Mathf.Deg2Rad;
            this.laserScan.time_increment  = 0.0;
            this.laserScan.scan_time       = 0.1;       // tentative
            this.laserScan.range_min       = 0.05;
            this.laserScan.range_max       = 20.0;
            this.laserScan.ranges          = new double[this.numLines];
            this.laserScan.intensities     = new double[this.numLines];

            this.laserScanMsg = new SIGVerseROSBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan);

//			Debug.Log("this.layerMask.value = "+this.layerMask.value);
        }
        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;
        }
Example #14
0
        public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isRight)
        {
            if (rosBridgeIP.Equals(string.Empty))
            {
                rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
            }
            if (sigverseBridgePort == 0)
            {
                sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
            }


            this.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort);
            this.tcpClientImage      = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort);

            this.networkStreamCameraInfo              = this.tcpClientCameraInfo.GetStream();
            this.networkStreamCameraInfo.ReadTimeout  = 100000;
            this.networkStreamCameraInfo.WriteTimeout = 100000;

            this.networkStreamImage              = this.tcpClientImage.GetStream();
            this.networkStreamImage.ReadTimeout  = 100000;
            this.networkStreamImage.WriteTimeout = 100000;


            // RGB Camera
            this.rgbCamera = this.cameraFrameObj.GetComponentInChildren <Camera>();

            int imageWidth  = this.rgbCamera.targetTexture.width;
            int imageHeight = this.rgbCamera.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 = { 968.765, 0.0, 640, 0.0, 968.77, 480, 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 = { 968.765, 0.0, 640, 0.0, 0.0, 968.77, 480, 0.0, 0.0, 0.0, 1.0, 0.0 };

            if (isRight)
            {
                P[3] = -135.627;                  // -135.627 = - 968.765 * 0.14(baseline=distance between both eyes)
            }

            //double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
            //double[] K = { 968.7653251755174, 0.0, 640.5, 0.0, 968.7653251755174, 480.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 };
            // Left Camera
            //double[] P = { 968.7653251755174, 0.0, 640.5, -0.0, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
            // Right Camera
            //double[] P = { 968.7653251755174, 0.0, 640.5, -135.62714552457246, 0.0, 968.7653251755174, 480.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), this.cameraFrameObj.name);

            this.cameraInfoMsg = new SIGVerseROSBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
            this.imageMsg      = new SIGVerseROSBridgeMessage <ImageForSIGVerseBridge>     ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
        }
Example #15
0
 void Awake()
 {
     this.baseFootPrint = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.BaseFootPrintName);
 }
 private static bool IsOverLimitSpeed(HSRCommon.Joint joint, double speed)
 {
     return(speed > HSRCommon.GetMaxJointSpeed(joint));
 }
        private void CheckOverLimitSpeed()
        {
            foreach (HSRCommon.Joint joint in this.trajectoryKeyList)
            {
                if (this.trajectoryInfoMap[joint] == null)
                {
                    continue;
                }

                List <float> trajectoryInfoDurations     = new List <float>(this.trajectoryInfoMap[joint].Durations);
                List <float> trajectoryInfoGoalPositions = new List <float>(this.trajectoryInfoMap[joint].GoalPositions);

                trajectoryInfoDurations.Insert(0, 0.0f);
                trajectoryInfoGoalPositions.Insert(0, this.trajectoryInfoMap[joint].CurrentPosition);

                for (int i = 1; i < trajectoryInfoGoalPositions.Count; i++)
                {
                    double tempDistance  = Math.Abs(trajectoryInfoGoalPositions[i] - trajectoryInfoGoalPositions[i - 1]);
                    double tempDurations = Math.Abs(trajectoryInfoDurations    [i] - trajectoryInfoDurations[i - 1]);
                    double tempSpeed     = tempDistance / tempDurations;

                    if (IsOverLimitSpeed(joint, tempSpeed))
                    {
                        SIGVerseLogger.Warn("Trajectry speed error. (" + this.topicName + ", Joint Name=" + joint.ToString() + ", Speed=" + tempSpeed + ", Max Speed=" + HSRCommon.GetMaxJointSpeed(joint) + ")");
                        return;
                    }
                }
            }
        }
 private void SetJointTrajectoryRotation(HSRCommon.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition)
 {
     this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, HSRCommon.GetNormalizedJointEulerAngle(currentPosition, joint) * Mathf.Deg2Rad);
 }
        protected override void SubscribeMessageCallback(SIGVerse.RosBridge.trajectory_msgs.JointTrajectory jointTrajectory)
        {
            if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
            {
                SIGVerseLogger.Warn("joint_names.Count != points.positions.Count  topicName = " + this.topicName);
                return;
            }

            for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
            {
                string name = jointTrajectory.joint_names[i];

                List <float> positions = new List <float>();
                List <float> durations = new List <float>();
                for (int pointIndex = 0; pointIndex < jointTrajectory.points.Count; pointIndex++)
                {
                    positions.Add(HSRCommon.GetClampedPosition((float)jointTrajectory.points[pointIndex].positions[i], name));
                    durations.Add((float)jointTrajectory.points[pointIndex].time_from_start.secs + (float)jointTrajectory.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
                }


                if (name == HSRCommon.ArmLiftJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
                }

                if (name == HSRCommon.ArmFlexJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.ArmRollJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.WristFlexJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.WristRollJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HeadPanJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HeadTiltJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HandLProximalJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HandRProximalJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
                }
            }
        }
Example #20
0
        public void JointTrajectoryCallback(SIGVerse.ROSBridge.trajectory_msgs.JointTrajectory jointTrajectory)
        {
            //List<string>  jointNames = jointTrajectory.joint_names;
            //List<SIGVerse.ROSBridge.trajectory_msgs.JointTrajectoryPoint> points = jointTrajectory.points;

            if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
            {
                SIGVerseLogger.Warn("joint_names.Count != points.positions.Count  topicName = " + this.topicName);
                return;
            }

            const int Zero = 0;

            for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
            {
                string name     = jointTrajectory.joint_names[i];
                float  position = HSRCommon.GetClampedPosition((float)jointTrajectory.points[Zero].positions[i], name);
                float  duration = (float)jointTrajectory.points[Zero].time_from_start.secs + (float)jointTrajectory.points[Zero].time_from_start.nsecs * 1.0e-9f;

//				Debug.Log("Duration="+ duration);

                if (name == HSRCommon.ArmLiftJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
                }

                if (name == HSRCommon.ArmFlexJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.ArmRollJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.WristFlexJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.WristRollJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HeadPanJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HeadTiltJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
                }


                if (name == HSRCommon.HandLProximalJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
                }

                if (name == HSRCommon.HandRProximalJointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
                }
            }
        }
Example #21
0
        void Update()
        {
            if (this.webSocketConnection == null || !this.webSocketConnection.IsConnected)
            {
                return;
            }

            foreach (string jointName in this.trajectoryKeyList)
            {
                if (this.trajectoryInfoMap[jointName] != null)
                {
                    if (jointName == HSRCommon.ArmLiftJointName)
                    {
                        float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeed, HSRCommon.MaxSpeedTorso);

                        this.armLiftLink.localPosition   = new Vector3(this.armLiftLink.localPosition.x, this.armLiftLink.localPosition.y, this.armLiftLinkIniPosZ + newPos);
                        this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos / 2.0f);
                    }

                    if (jointName == HSRCommon.ArmFlexJointName)
                    {
                        float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);

                        this.armFlexLink.localEulerAngles = new Vector3(this.armFlexLink.localEulerAngles.x, newPos, this.armFlexLink.localEulerAngles.z);
                    }

                    if (jointName == HSRCommon.ArmRollJointName)
                    {
                        float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);

                        this.armRollLink.localEulerAngles = new Vector3(this.armRollLink.localEulerAngles.x, this.armRollLink.localEulerAngles.y, newPos);
                    }

                    if (jointName == HSRCommon.WristFlexJointName)
                    {
                        float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);

                        this.wristFlexLink.localEulerAngles = new Vector3(this.wristFlexLink.localEulerAngles.x, newPos, this.wristFlexLink.localEulerAngles.z);
                    }

                    if (jointName == HSRCommon.WristRollJointName)
                    {
                        float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);

                        this.wristRollLink.localEulerAngles = new Vector3(this.wristRollLink.localEulerAngles.x, this.wristRollLink.localEulerAngles.y, newPos);
                    }

                    if (jointName == HSRCommon.HeadPanJointName)
                    {
                        float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedHead) * Mathf.Rad2Deg, jointName);

                        this.headPanLink.localEulerAngles = new Vector3(this.headPanLink.localEulerAngles.x, this.headPanLink.localEulerAngles.y, newPos);
                    }

                    if (jointName == HSRCommon.HeadTiltJointName)
                    {
                        float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedHead) * Mathf.Rad2Deg, jointName);

                        this.headTiltLink.localEulerAngles = new Vector3(this.headTiltLink.localEulerAngles.x, newPos, this.headTiltLink.localEulerAngles.z);
                    }

                    if (jointName == HSRCommon.HandLProximalJointName)
                    {
                        float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);

                        // Grasping and hand closing
                        if (this.graspedObject != null && this.IsAngleDecreasing(newPos, this.handLProximalLink.localEulerAngles.x))
                        {
                            // Have to stop
                            this.trajectoryInfoMap[jointName] = null;
                        }
                        // Otherwise
                        else
                        {
                            this.handLProximalLink.localEulerAngles = new Vector3(newPos, this.handLProximalLink.localEulerAngles.y, this.handLProximalLink.localEulerAngles.z);
                        }
                    }

                    if (jointName == HSRCommon.HandRProximalJointName)
                    {
                        float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);

                        // Grasping and hand closing
                        if (this.graspedObject != null && this.IsAngleIncreasing(newPos, this.handRProximalLink.localEulerAngles.x))
                        {
                            // Have to stop
                            this.trajectoryInfoMap[jointName] = null;
                        }
                        // Otherwise
                        else
                        {
                            this.handRProximalLink.localEulerAngles = new Vector3(newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
                        }
                    }
                }
            }

            this.webSocketConnection.Render();
        }
Example #22
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.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
            this.tcpClientImage      = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);

            this.networkStreamCameraInfo              = this.tcpClientCameraInfo.GetStream();
            this.networkStreamCameraInfo.ReadTimeout  = 100000;
            this.networkStreamCameraInfo.WriteTimeout = 100000;

            this.networkStreamImage              = this.tcpClientImage.GetStream();
            this.networkStreamImage.ReadTimeout  = 100000;
            this.networkStreamImage.WriteTimeout = 100000;

            // RGB Camera
            this.xtionRGBCamera = this.rgbCamera.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 = { 554, 0.0, 320, 0.0, 554, 240, 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 = { 554, 0.0, 320, 0.0, 0.0, 554, 240, 0.0, 0.0, 0.0, 1.0, 0.0 };

            //double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
            //double[] K = { 554.3827128226441, 0.0, 320.5, 0.0, 554.3827128226441, 240.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 = { 554.3827128226441, 0.0, 320.5, 0.0, 0.0, 554.3827128226441, 240.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), this.rgbCamera.name);


            this.cameraInfoMsg = new SIGVerseROSBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
            this.imageMsg      = new SIGVerseROSBridgeMessage <ImageForSIGVerseBridge>     ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
        }
Example #23
0
        protected override void SubscribeMessageCallback(SIGVerse.RosBridge.trajectory_msgs.JointTrajectory jointTrajectory)
        {
            this.startPosition = this.baseFootprintRigidbody.position;//save start position.
            this.startRotation = this.baseFootprintRigidbody.rotation;


            if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
            {
                SIGVerseLogger.Warn("joint_names.Count != points.positions.Count  topicName = " + this.topicName);
                return;
            }
            else if (jointTrajectory.joint_names.Count != 3)
            {
                SIGVerseLogger.Warn("joint_names.Count != 3");
                return;
            }

            for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
            {
                string name = jointTrajectory.joint_names[i];

                List <float> positions = new List <float>();
                List <float> durations = new List <float>();
                for (int pointIndex = 0; pointIndex < jointTrajectory.points.Count; pointIndex++)
                {
                    positions.Add(HSRCommon.GetClampedPosition((float)jointTrajectory.points[pointIndex].positions[i], name));
                    durations.Add((float)jointTrajectory.points[pointIndex].time_from_start.secs + (float)jointTrajectory.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
                }

                if (name == HSRCommon.OmniOdomX_JointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.baseFootprintRigidbody.position.z);
                }

                if (name == HSRCommon.OmniOdomY_JointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.baseFootprintRigidbody.position.x);
                }

                if (name == HSRCommon.OmniOdomT_JointName)
                {
                    this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, 0.0f);
                }
            }

            if (trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] == null || trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] == null || trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] == null)
            {
                trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
                trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
                trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] = null;
                SIGVerseLogger.Warn("Omni trajectry args error.");
            }

            //check limit speed
            for (int i = 0; i < trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions.Count; i++)
            {
                double linear_speed = 0;
                if (i == 0)
                {
                    double temp_distance = Math.Sqrt(Math.Pow(((trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[0] + this.initialPosition.z) - this.startPosition.z) * 100, 2) + Math.Pow(((trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[0] + this.initialPosition.x) - this.startPosition.x) * 100, 2));
                    linear_speed = (temp_distance / 100) / trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[0];
                }
                else
                {
                    double temp_distance = Math.Sqrt(Math.Pow((trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[i] - trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[i - 1]) * 100, 2) + Math.Pow((trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[i] - trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[i - 1]) * 100, 2));
                    linear_speed = (temp_distance / 100) / (trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[i] - trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[i - 1]);
                }

                double angular_speed = 0;
                if (i == 0)
                {
                    double temp_angle = Math.Abs((this.ToAngle(trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[0]) + this.initialRotation.eulerAngles.y) - this.startRotation.eulerAngles.y);
                    if (temp_angle > Math.PI)
                    {
                        temp_angle -= Math.PI;
                    }
                    angular_speed = temp_angle / trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[0];
                }
                else
                {
                    double temp_angle = Math.Abs(this.ToAngle(trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[i]) - trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[i - 1]);
                    if (temp_angle > Math.PI)
                    {
                        temp_angle -= Math.PI;
                    }
                    angular_speed = temp_angle / (trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[i] - trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[i - 1]);
                }

                if (linear_speed > HSRCommon.MaxSpeedBase || angular_speed > this.ToAngle(HSRCommon.MaxSpeedBaseRad))//exceed limit
                {
                    trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
                    trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
                    trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] = null;
                    SIGVerseLogger.Warn("Omni trajectry args error. (exceed limit)");
                    return;
                }
            } //for
        }     //SubscribeMessageCallback