private static bool IsOverLimitSpeed(PR2Common.Joint joint, double speed)
{
return(speed > PR2Common.GetMaxJointSpeed(joint));
}
private static float GetPositionAndUpdateTrajectory(Dictionary <PR2Common.Joint, TrajectoryInfo> trajectoryInfoMap, PR2Common.Joint joint)
{
float minSpeed = PR2Common.GetMinJointSpeed(joint);
float maxSpeed = PR2Common.GetMaxJointSpeed(joint);
TrajectoryInfo trajectoryInfo = trajectoryInfoMap[joint];
int targetPointIndex = GetTargetPointIndex(trajectoryInfo);
float goalPosition = trajectoryInfo.GoalPositions[targetPointIndex];
if (PR2Common.continuousJoints.Contains(joint))
{
if (goalPosition - trajectoryInfo.CurrentPosition > +Mathf.PI)
{
goalPosition -= 2 * Mathf.PI;
}
if (goalPosition - trajectoryInfo.CurrentPosition <= -Mathf.PI)
{
goalPosition += 2 * Mathf.PI;
}
}
float speed = 0.0f;
if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Durations[targetPointIndex])
{
speed = maxSpeed;
}
else
{
speed = Mathf.Abs((goalPosition - 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(goalPosition - trajectoryInfo.CurrentPosition))
{
newPosition = goalPosition;
trajectoryInfoMap[joint] = null;
}
else
{
trajectoryInfo.CurrentTime = Time.time;
if (goalPosition > trajectoryInfo.CurrentPosition)
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance;
}
else
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance;
}
if (PR2Common.continuousJoints.Contains(joint))
{
trajectoryInfo.CurrentPosition = GetNormalizedRadian(trajectoryInfo.CurrentPosition);
}
newPosition = trajectoryInfo.CurrentPosition;
}
return(newPosition);
}