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 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; } } } }