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