Log in or register to post comments

Robot (EE) to track augmented sphere (Inverse Kinematics)

December 22, 2020 - 8:33pm #1

Hi everyone!

Just wanted to obtain some pointers as to how do we enable the robot's EE to track the augmented sphere using Vuforia. I notice it works if I run it within the unity environment but not for AR as the EE would head the opposite direction instead. Attached below are the codes.

IK Script for Robot:

using UnityEngine;

// ReSharper disable once InconsistentNaming
public class IKController : MonoBehaviour


{
    [SerializeField] private Transform _targetTransform;
    public ArmJoint[] Joints;
    public float[] Angles;

    private const float SamplingDistance = 0.1f;
    private const float LearningRate = 0.01f;
    private const float DistanceThreshold = 0.1f;




    private void Start()  
 
    {
        float[] angles = new float[Joints.Length];
     
        for (int i = 0; i < Joints.Length; i++) {
            if (Joints[i]._rotationAxis == 'x') {
                angles[i] = Joints[i].transform.localRotation.eulerAngles.x;
            }
            else if (Joints[i]._rotationAxis == 'y') {
                angles[i] = Joints[i].transform.localRotation.eulerAngles.y;
            }
            else if (Joints[i]._rotationAxis == 'z') {
                angles[i] = Joints[i].transform.localRotation.eulerAngles.z;
            }
        }
        Angles = angles;
    }



    private void Update()
    {
        InverseKinematics(_targetTransform.position, Angles);
    }




    public Vector3 ForwardKinematics(float[] angles)
    {
        Vector3 prevPoint = Joints[0].transform.position;
        Quaternion rotation = Quaternion.identity;
        for (int i = 1; i < Joints.Length; i++) {
            // Rotates around a new axis
            rotation *= Quaternion.AngleAxis(angles[i - 1], Joints[i - 1].RotationAxis);
            Vector3 nextPoint = prevPoint + rotation * Joints[i].StartOffset;

            prevPoint = nextPoint;
        }
        return prevPoint;
    }

    //To obtain distance from target
    public float DistanceFromTarget(Vector3 target, float[] angles)
    {
        Vector3 point = ForwardKinematics(angles);
        return Vector3.Distance(point, target);
    }

    //function that estimates the partial gradient of i-th joint
    public float PartialGradient(Vector3 target, float[] angles, int i)
    {
        // Saves the angle,
        // it will be restored later
        float angle = angles[i];

        // Gradient : [F(x+SamplingDistance) - F(x)] / h
        float f_x = DistanceFromTarget(target, angles);

        angles[i] += SamplingDistance;
        float f_x_plus_d = DistanceFromTarget(target, angles);

        float gradient = (f_x_plus_d - f_x) / SamplingDistance;

        // Restores
        angles[i] = angle;

        return gradient;
    }


    //loop over the other joints to see their contribution to the gradient
    public void InverseKinematics(Vector3 target, float[] angles)
    {
        if (DistanceFromTarget(target, angles) < DistanceThreshold) //early termination to prevent overshoot of the robot when angles are update too many times
            return;

        for (int i = Joints.Length - 1; i >= 0; i--) {
            // Gradient descent
            // Update : Solution -= LearningRate * Gradient
            float gradient = PartialGradient(target, angles, i);
            angles[i] -= LearningRate * gradient;

            // Early termination
            if (DistanceFromTarget(target, angles) < DistanceThreshold)
                return;
         
            switch (Joints[i]._rotationAxis) {
                case 'x':
                    Joints[i].transform.localEulerAngles = new Vector3(angles[i], 0, 0);
                    break;
                case 'y':
                    Joints[i].transform.localEulerAngles = new Vector3(0, angles[i], 0);
                    break;
                case 'z':
                    Joints[i].transform.localEulerAngles = new Vector3(0, 0, angles[i]);
                    break;
            }
        }
    }
}

 

Sphere (Endpoint) that works within the unity environment:

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;

public class ControlEndpoint : MonoBehaviour {
  
    public GameObject endPoint, LT;
    public int speed = 1000;

// Use this for initialization
void Start () {

    }

    void Update()
    {
      
    }

    public void MoveUp()
    {
      
        endPoint.transform.Translate(Vector3.up * Time.deltaTime * speed);
      
    }
    public void MoveDown()
    {

        endPoint.transform.Translate(Vector3.down * Time.deltaTime * speed);
      
    }
    public void MoveRight()
    {

        endPoint.transform.Translate(Vector3.right * Time.deltaTime * speed);
      
    }
    public void MoveLeft()
    {

        endPoint.transform.Translate(Vector3.left * Time.deltaTime * speed);

    }
    public void MoveForward()
    {

        endPoint.transform.Translate(Vector3.forward * Time.deltaTime * speed);

    }
    public void MoveBack()
    {

        endPoint.transform.Translate(Vector3.back * Time.deltaTime * speed);

    }
    public void resetOrigin()
    {
        endPoint.transform.localPosition = new Vector3(-115.0f, 0, 0);
        endPoint.transform.localEulerAngles = new Vector3(0, 0, 0);
    }

    public void xaxiscw()
    {
        endPoint.transform.Rotate(Vector3.left * Time.deltaTime * speed);
    }
    public void xaxisccw()
    {
        endPoint.transform.Rotate(Vector3.right * Time.deltaTime * speed);
    }
    public void yaxiscw()
    {
        endPoint.transform.Rotate(Vector3.up * Time.deltaTime * speed);
    }
    public void yaxisccw()
    {
        endPoint.transform.Rotate(Vector3.down * Time.deltaTime * speed);
    }
    public void zaxiscw()
    {
        endPoint.transform.Rotate(Vector3.forward * Time.deltaTime * speed);
    }
    public void zaxisccw()
    {
        endPoint.transform.Rotate(Vector3.back * Time.deltaTime * speed);
    }

    // Update is called once per frame
    void FixedUpdate () {

        if (Input.GetKey(KeyCode.LeftArrow)) { MoveForward();  }
        if (Input.GetKey(KeyCode.RightArrow)) { MoveBack(); }
        if (Input.GetKey(KeyCode.UpArrow)) { MoveRight(); }
        if (Input.GetKey(KeyCode.DownArrow)) { MoveLeft(); }
        if (Input.GetKey(KeyCode.RightShift)) { MoveUp(); }
        if (Input.GetKey(KeyCode.RightControl)) { MoveDown(); }

        if (Input.GetKey(KeyCode.Keypad1)) { xaxiscw(); }
        if (Input.GetKey(KeyCode.Keypad4)) { xaxisccw(); }
        if (Input.GetKey(KeyCode.Keypad2)) { yaxiscw(); }
        if (Input.GetKey(KeyCode.Keypad5)) { yaxisccw(); }
        if (Input.GetKey(KeyCode.Keypad3)) { zaxiscw(); }
        if (Input.GetKey(KeyCode.Keypad6)) { zaxisccw(); }

        if (Input.GetKey(KeyCode.A)) { LT.transform.Translate(Vector3.back * Time.deltaTime * speed); }
        if (Input.GetKey(KeyCode.Z)) { LT.transform.Translate(Vector3.forward * Time.deltaTime * speed); }

        //if (Input.GetKey(KeyCode.Space)) { resetOrigin(); }
    }
  
}

 

The script for each joint:

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class ArmJoint : MonoBehaviour {
    public Vector3 RotationAxis;
    public Vector3 StartOffset;
    private Transform _transform;
    public char _rotationAxis;

    private void Awake() {
        _transform = this.transform;
        StartOffset = _transform.localPosition;
    }
}

 

It would be great if it works within the AR environment using Vuforia as shown in the image attached (i.e. Drag the EE using a teach pendant) and I would appreciate any inputs.

 

Regards.

AttachmentSize
Image icon AR sphere.png142.05 KB
Log in or register to post comments