Custom Player Controller Ground Warping *SOLVED*

I am making my own player controller, and an important part is warping to the ground so you don't go flying off the ground every time you get to the top of a hill. A sweeptest or raycast or spherecast can easily be used and you just change the player's y position to place them back on the ground nicely. Unfortunately, for me its not that easy because I'm making a player controller that is much like the one in Super Mario Galaxy. So, the problem is that I'm having undesired effects when attempting to warp the player. If anyone can help me fix this that would be great! Images of undesired effect as well as ground warping code below. A demo is on my site, linked below. I also added the entire script in case but not that the top script is just a part of the bottom one.

 if (!jumping && !launched) {
     if (links.r.SweepTest(-links.t.up, out hitInfo, groundCheckDistance)) {
         links.t.position = hitInfo.point + (links.t.up * _INPUT_PlayerHeight * .5f);
         movementCalculation = new Vector3(movementCalculation.x, 0, movementCalculation.z);
         grounded = true;
     }
 }

NOTE: jumping and launched are false in all the pictures, links.t is the transform of the player, _INPUT_PlayerHeight is the height of the player capsule collider, movementCalculation is what I set the players rigidbody later in the script and the movementCalculation (before modified by the warping section shown) is set to the players rigidbody velocity.

http://prntscr.com/6i09o1 NOTE: Holding W to move forward but the player is not moving. The warp is freezing it there. Also NOTE (not shown in the picture): When the player is on a gameobject, if close enoguh, they tend to slide to the edges but then not fall off.

I have the highlighted problem part of the script abover, but just in case, I will post the entire script.

 using UnityEngine;
 using System.Collections;
 
 public class PlayerMovementManager : MonoBehaviour {
 
     //Preference Variables
     public float walkSpeed = 8f;
     public float walkAcceleration = 1f, inAirWalkAcceleration = .5f;//Change default
     public float deacceleration = .5f, inAirDeacceleration = .95f;
     public float maxSlope = 70f;
     //public float terminalVelocity = 100f;
     public float jumpForce = 6f;
     public float groundCheckDistance = .15f;
 
     //Script Variables (changed upon use in the script)
     [HideInInspector] public bool grounded;//Need to be accesed by input;
     [HideInInspector] public bool launched;
     private int launchCooldown;
     //private int airTime;//Every update that the player is aurborne this gets +1.
     private RaycastHit hitInfo;
     private bool jumping;
     private Vector3 movementCalculation, horizontalMovementCalculation;
     private float inAirMovementMagnitude;
     private float diagonalAspect = 0.7071068f;
 
     //Link
     private PlayerLinkManager links;
     
     //Input Variables
     [HideInInspector] public float _INPUT_X, _INPUT_Z;
     [HideInInspector] public Vector3 _INPUT_GravityVector;
     [HideInInspector] public bool _INPUT_JumpInitiate;
     [HideInInspector] public float _INPUT_PlayerHeight;
 
     void Start () {
         links = gameObject.GetComponent<PlayerLinkManager>();
     }
 
     public void UpdateMovement () {
         movementCalculation = links.t.InverseTransformDirection(links.r.velocity);
         horizontalMovementCalculation = new Vector3(movementCalculation.x, 0, movementCalculation.z);
         launchCooldown--;
 
         //Sprint
 
         //Ground warp
         if (!jumping && !launched) {
             if (links.r.SweepTest(-links.t.up, out hitInfo, groundCheckDistance)) {//if (links.r.SweepTest(-links.t.up, out hitInfo, groundCheckDistance)) {     //if (Physics.Raycast(links.t.position, -links.t.up, out hitInfo, groundCheckDistance + (_INPUT_PlayerHeight * 0.5f)))
                 if (true) {//hitInfo.transform.tag == "Planet"
                     links.t.position = hitInfo.point + (links.t.up * _INPUT_PlayerHeight * .5f);
                     movementCalculation = new Vector3(movementCalculation.x, 0, movementCalculation.z);
                     grounded = true;
                 }
             }
         }
 
         //WASD to Movement Calculations
         if (grounded) {
             if (_INPUT_X != 0 && _INPUT_Z != 0) {
                 movementCalculation += new Vector3(_INPUT_X * walkAcceleration * diagonalAspect, 0, _INPUT_Z * walkAcceleration * diagonalAspect);
             } else if (_INPUT_X != 0 || _INPUT_Z != 0) {
                 movementCalculation += new Vector3(_INPUT_X * walkAcceleration, 0, _INPUT_Z * walkAcceleration);
             } else {
                 //Deacceleration
                 movementCalculation = new Vector3(movementCalculation.x * deacceleration, movementCalculation.y, movementCalculation.z * deacceleration);
             }
         } else {
             if (horizontalMovementCalculation.magnitude > walkSpeed) {
                 inAirMovementMagnitude = horizontalMovementCalculation.magnitude;
             } else {
                 inAirMovementMagnitude = walkSpeed;
             }
             if (_INPUT_X != 0 && _INPUT_Z != 0) {
                 movementCalculation += new Vector3(_INPUT_X * inAirWalkAcceleration * diagonalAspect, 0, _INPUT_Z * inAirWalkAcceleration * diagonalAspect);
             } else if (_INPUT_X != 0 || _INPUT_Z != 0) {
                 movementCalculation += new Vector3(_INPUT_X * inAirWalkAcceleration, 0, _INPUT_Z * inAirWalkAcceleration);
             } else {
                 //Deacceleration
                 movementCalculation = new Vector3(movementCalculation.x * inAirDeacceleration, movementCalculation.y, movementCalculation.z * inAirDeacceleration);
             }
             horizontalMovementCalculation = new Vector3(movementCalculation.x, 0, movementCalculation.z);
             if (horizontalMovementCalculation.magnitude > inAirMovementMagnitude) {
                 horizontalMovementCalculation = horizontalMovementCalculation.normalized * inAirMovementMagnitude;
                 movementCalculation = new Vector3(horizontalMovementCalculation.x, movementCalculation.y, horizontalMovementCalculation.z);
             }
         }
         horizontalMovementCalculation = new Vector3(movementCalculation.x, 0, movementCalculation.z);
 
         //Movement Magnitude Correction Calculations
         if (grounded) {
             if (horizontalMovementCalculation.magnitude > walkSpeed) {
                 horizontalMovementCalculation = horizontalMovementCalculation.normalized * walkSpeed;
                 movementCalculation = new Vector3(horizontalMovementCalculation.x, movementCalculation.y, horizontalMovementCalculation.z);
             }
         }
 
         //Jump Calculations
         if (_INPUT_JumpInitiate) {
             movementCalculation = new Vector3(movementCalculation.x, jumpForce, movementCalculation.z);;
             jumping = true;
             grounded = false;
             _INPUT_JumpInitiate = false;
         }
 
         //Apply Movement Calculatuons
         if (!launched)
             links.r.velocity = links.t.TransformDirection(movementCalculation);
 
         //Gravity
         if (!grounded) {
             links.r.velocity += _INPUT_GravityVector * links.playerManager.gravityIntensity;
         }
     }
 
     void OnCollisionEnter(Collision collision) {
         foreach (ContactPoint contact in collision.contacts) {
             if (Vector3.Angle(contact.normal, links.t.up) < maxSlope) {
                 jumping = false;
                 grounded = true;
                 if (launchCooldown <= 0) {
                     launched = false;
                 }
             }
         }
     }
 
     void OnCollisionExit(Collision Collision) {
         grounded = false;
 
     }
 
     public void Launch(Vector3 launchDirection, float force) {//Called by touching launch pads
         launched = true;
         launchCooldown = 10;
         links.r.velocity = launchDirection * force;
     }
 }

Also, because I think this will help a lot, I have put a demo on my website. It can be found here: http://www.enragedstudios.com/DEMO/BUILD.html . NOTE: The red platforms are launchers so they will have that effect if you step on them. I figured I would add this not so you know its not a bug in warping.

If you need more information, please ask. I have posted this question because I have been trying to get this to work far longer than I hoped too, and cannot do it on my own. I tried doing research and using the api and in most cases I solve my own issue, but I require outside help! Thanks :D!