/// <summary>
/// Runs during the physics loop at 250 Hz (fixed timestep of 0.004 s). Typically, robot
/// control (receiving positions and sending forces) should happen here.
///
/// To change the loop rate, open the Unity editor and go to the Time Manager
/// (menu: Edit > Project Settings > Time) and change the value of "Fixed Timestep". At
/// this time, we recommend a maximum loop rate of 400 Hz (minimum timestep 0.0025 s).
/// Any time you change the loop rate, you should also re-tune any control gains
/// (including the stiffness and damping of haptic objects).
/// Reference: https://docs.unity3d.com/Manual/class-TimeManager.html
/// </summary>
void FixedUpdate()
{
tool_position = positionScale * robot_left.GetToolPosition();
tool_velocity = positionScale * robot_left.GetToolVelocity();
transform.position = tool_position;
/// Dividing the force by positionScale allows the gains to be independent of the value
/// of positionScale. If you add forces that are not related to haptic objects and are
/// independent of the scaling, you may need to divide only certain components of the
/// force by positionScale.
robot_left.SetToolForce(tool_force / positionScale);
}
void FixedUpdate()
{
tool_position = positionScale * robot.GetToolPosition();
tool_velocity = positionScale * robot.GetToolVelocity();
transform.position = tool_position;
rb.position = new Vector3
(
Mathf.Clamp(rb.position.x + xPosOffset, boundary.xmin, boundary.xmax),
0.0f,
Mathf.Clamp(rb.position.z + zPosOffset, boundary.zmin, boundary.zmax)
);
rb.rotation = Quaternion.Euler(0, 0, tool_velocity[0] * -tilt);
if (dampingFlag)
{
tool_force = -damping * tool_velocity;
}
else
{
tool_force = Vector3.zero;
}
robot.SetToolForce(tool_force / positionScale);
}
