예제 #1
0
    public BurtSharp.CoAP.MsgTypes.RobotCommand CalcForces()
    {
        Vector3 vel = _currentVelocity;
        Vector3 pos = _currentPosition;



        Vector3 vel_norm = vel.normalized;
        float   vel_mag  = vel.magnitude;

        Vector3 coreTorque = Vector3.zero;


        //email felix
        //this section is what we want
        _teneoTorque0 = 1.0f;
        //CalibTenoAngle = -Mathf.DeltaAngle(handangles[2], CalibAngle1) + 180.0f;

        //relates torque linearly to difference in angle between player and some arbitrary constant
        CalibTenoAngle = -Mathf.DeltaAngle(handangles[3], CalibAngle1) + 180.0f; //apply calibration to pronation/supination portion of handangles

        if (CalibTenoAngle > 160 + 180)
        {
            //Debug.Log ("Teneo Limit high: "  );

            _teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 160 + 180) * Mathf.PI / 180), 0, 1);
            //_teneoTorque = 0f;
        }

        if (CalibTenoAngle < 20 + 180)
        {
            //Debug.Log ("Teneo Limit low: "  );
            //Mathf.Cos((handangles[2]-180+90))
            _teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 20 + 180) * Mathf.PI / 180), 0, 1);
            //_teneoTorque = 0f;
        }


        _toolForceQ   = _toolForce;
        _teneoTorqueQ = _teneoTorque;

        //we want to apply different viscosities
        //positive viscosity F = -bx
        //negative viscosity F = bx
        //where b is the PSFactor
        //x is the user torque/hand velocity
        //need safety checks in place as well

        _teneoTorque = _teneoTorque0

                       return(RobotCommandExtensions.RobotCommandForceTorque(_toolForce * MasterForce, coreTorque, _teneoTorque * MasterForce));
    }
    public BurtSharp.CoAP.MsgTypes.RobotCommand CalcForces()
    {
        Vector3 vel = _currentVelocity;
        Vector3 pos = _currentPosition;



        Vector3 vel_norm = vel.normalized;
        float   vel_mag  = vel.magnitude;

        Vector3 coreTorque = Vector3.zero;



        //_teneoTorque0 = 1.0f;
        CalibTenoAngle = -Mathf.DeltaAngle(handangles [2], CalibAngle1) + 180.0f;

        if (CalibTenoAngle > 160 + 180)
        {
            //Debug.Log ("Teneo Limit high: "  );

            _teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 160 + 180) * Mathf.PI / 180), 0, 1);
            //_teneoTorque = 0f;
        }

        if (CalibTenoAngle < 20 + 180)
        {
            //Debug.Log ("Teneo Limit low: "  );
            //Mathf.Cos((handangles[2]-180+90))
            _teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 20 + 180) * Mathf.PI / 180), 0, 1);
            //_teneoTorque = 0f;
        }


        _toolForceQ   = _toolForce;
        _teneoTorqueQ = _teneoTorque;

        return(RobotCommandExtensions.RobotCommandForceTorque(_toolForce * MasterForce, coreTorque, _teneoTorque * MasterForce));
    }
예제 #3
0
    /// <summary>
    /// This is the control loop that calculates the forces to be applied to the robot. It
    /// runs automatically through RobotConnection, as long as it is registered. In this
    /// example, this function is registered in the Start () function.
    ///
    /// The RobotCommand type may include a force, a torque, both, or neither. Whichever of
    /// these are set get sent to the robot.
    /// </summary>
    public BurtSharp.CoAP.MsgTypes.RobotCommand CalcForces()
    {
        /// Dividing the force by kPositionScale allows the gains to be independent of the value
        /// of kPositionScale. 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 kPositionScale.
        //        Vector3 force = _toolForce / kPositionScale;
        Vector3 vel = _currentVelocity;
        Vector3 pos = _currentPosition;



        Vector3 hand_to_target_vec      = target_pos - pos;
        Vector3 hand_to_target_vec_norm = hand_to_target_vec.normalized;

        Vector3 vel_norm = vel.normalized;
        float   vel_mag  = vel.magnitude;
        //Vector3 plane_norm = Vector3.Cross(vel_norm, hand_to_target_vec_norm);

        //Vector3 force_norm = Vector3.Cross(hand_to_target_vec_norm, plane_norm);

        //float proj_angle = Mathf.Acos (Vector3.Dot(vel_norm, hand_to_target_vec_norm));

        //float forcemag = 3.0f*vel_mag*Mathf.Sin(proj_angle);
        //Vector3 force1 = forcemag*force_norm;



        //***CREATE FORCE AND TORQUE USING EA***

        //force increases linearly with distance from player game object to target
        Vector3 force2 = -EA_gain * hand_to_target_vec;

        //* Mathf.PI / 180.0f

        //torque increases linearly with difference in angle between player game object and target
        _teneoTorque0 = -EA_gain * 0.002f * Mathf.DeltaAngle(target_angles[2], handangles[2]);
        //(0.0f*target_angles [2] + 1.0f*transform.eulerAngles[2] );

        //Vector3 force = _robot.GetToolVelocity () *0 +2.0f;
        //_toolForce =new Vector3 (10,0,0);
        _toolForce = force2;
        //Debug.Log (target_pos.ToString ());
        Vector3 coreTorque = Vector3.zero;



        //_teneoTorque0 = 1.0f;
        CalibTenoAngle = -Mathf.DeltaAngle(handangles[2], CalibAngle1) + 180.0f;

        if (CalibTenoAngle > 160 + 180)
        {
            //Debug.Log ("Teneo Limit high: "  );

            //do not apply greater torques when the difference of angle between the target and hand is greater than 160
            _teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 160 + 180) * Mathf.PI / 180), 0, 1);
            //_teneoTorque = 0f;
        }

        if (CalibTenoAngle < 20 + 180)
        {
            //Debug.Log ("Teneo Limit low: "  );
            //Mathf.Cos((handangles[2]-180+90))

            //do not apply lesser torques when the difference of angle between the target and hand is less than 20
            _teneoTorque = _teneoTorque0 * Mathf.Clamp(Mathf.Cos(5.0f * (CalibTenoAngle - 20 + 180) * Mathf.PI / 180), 0, 1);
            //_teneoTorque = 0f;
        }

        //_teneoTorque = _teneoTorque0;


        //UnityEngine.Debug.Log (_toolForce.ToString ());
        _toolForceQ   = _toolForce;
        _teneoTorqueQ = _teneoTorque;


        //_robot.SetUserGravityCompGain (value, _ugcUnits)
        //_robot.SetUserGravityCompGain
        return(RobotCommandExtensions.RobotCommandForceTorque(_toolForce * MasterForce, coreTorque, _teneoTorque * MasterForce));
    }