void FixedUpdate()
{
if (!_robotDisabled)
{
if (!_finshedTrajectory)
{
if (_trajectory.startTime + _nextPoint.timeFromStart <= Time.fixedTime)
{
// while loop goes to the next point in trajectory, skipping points according to their timeFromStart value
while (_trajectory.startTime + _nextPoint.timeFromStart <= Time.fixedTime)
{
_pointIndex++;
if (_pointIndex == _trajectory.points.Count)
{
_finshedTrajectory = true;
float timeComplete = Time.fixedTime - _trajectory.startTime;
Debug.Log("Completed trajectory in: " + timeComplete.ToString("F3") + " seconds");
break;
}
_nextPoint = _trajectory.points[_pointIndex];
}
_goingToPoint = false;
}
if (!_goingToPoint)
{
FollowNextPoint();
_goingToPoint = true;
}
}
}
}
void FixedUpdate()
{
if (!_robot_disabled)
{
if (!_finished_traj)
{
if (_traj.start_time + _next_point.time_from_start <= Time.fixedTime)
{
// while loop goes to the next point in trajectory, skipping points according to their time_from_start value
while (_traj.start_time + _next_point.time_from_start <= Time.fixedTime)
{
_traj_index++;
if (_traj_index == _traj.points.Count)
{
_finished_traj = true;
float time_complete = Time.fixedTime - _traj.start_time;
Debug.Log("Completed trajectory in: " + time_complete.ToString("F3") + " seconds");
break;
}
_next_point = _traj.points[_traj_index];
}
_going_to_point = false;
}
if (!_going_to_point)
{
followNextPoint();
_going_to_point = true;
}
}
}
}
// sets the current joint trajectory to follow
public bool FollowTrajectory(JointTrajectory traj)
{
if (traj.points.Count == 0)
{
return(false);
}
_pointIndex = 0;
_trajectory = traj;
_nextPoint = _trajectory.points[_pointIndex];
_goingToPoint = false;
_finshedTrajectory = false;
return(true);
}
// sets the current joint trajectory to follow
public bool followTrajectory(JointTrajectory traj)
{
if (traj.points.Count == 0)
{
return(false);
}
_traj_index = 0;
_traj = traj;
_next_point = _traj.points[_traj_index];
_going_to_point = false;
_finished_traj = false;
return(true);
}
