Inverse Kinametics

damped_least_square.py

import numpy as np
import collections

def IK(joint, JOINT_IDs, Dmax=0.3, lam=0.1, a=0.1):
  [u, D, vT] = np.linalg.svd(jacobian(JOINT_IDs))
  op = vT.T @ np.diag(D / (D ** 2 + lam ** 2)) @ l.T
  
  th = clipped_vector(joint[JOINT_ID_EE]['p_des'] - joint[JOINT_ID_EE]['p'], Dmax) # p/p_des: desired/current Position in World Coord
  a = skew2axis(joint[JOINT_ID_EE]['R_des'] @ joint[JOINT_ID_EE]['R'].T) # R/R_des : desired/current attitude in World Coord
  q = a * op @ np.vstack((th, a))
  
  for idx, jdx in enumerate(JOINT_ID_ALL):
    joint[jdx]['q'] += q[idx] # q: joint angle
    
def jacobian(joint, IDs):
  J = np,zeros((6, len(IDs)))
  p_n = joint[IDs[-1]]['p'] # p: Position in World Coord
  for idx, j_id in enumerate(IDs):
    w = joint[j_id]['a'] # a: Joint Axis Vector rel to Parent
    p = joint[j_id]['p'] # p: Position in World Coord
    
    J[0:3, idx:idx+1] = np.cross(w, (p_n - p), axis=0)
    J[3:6, idx:idx+1] = w
  return J

def clipped_vector(vec, bound=None):
  norm = np.linalg.norm(vec) + 1.e-16
  bound = bound or norm
  return vec / norm * np.min((norm, bound))

def skew2axis(S):
  return np.array([[(S[2,1] - S[1,2])/2], [(S[0,2] - S[2,0])/2], [(S[1,0] - S[0,1])/2]])


if __name__ == '__main__':
  test_IK()
  

optimization.py

__author__ = "Hosik Chae"
__date__ = "22. 12. 7."
__copyright__ = "Copyright 2022, RoMeLa"
__version__ = "0.0.0"
__email__ = "[email protected]"
__status__ = "Development"


import time
import sys;sys.path.append('/home/pong0923/dev/lib/rbdl-orb/rbdl-build/python'); import rbdl
import os
import limms as _limms
from limms.settings import config
from limms.util.memory import Memory
from limms.util import HardwareBridge, robot, gazebo_bridge
from limms.constants import dynamixel as kD, joints as kJ, physical as kP
from limms.util.dual_shock4 import MEMORY_NAME, KEY_BUTTON, GAMEPAD_KEY, wait, DEFAULT_TIMEFUNC
from limms.util.kinematics import wrap_mirror

import numpy as np
import scipy as sp
import scipy.optimize
import scipy.spatial.transform.rotation as scipyr

import matplotlib.pyplot as plt

np.set_printoptions(precision=5, edgeitems=10, linewidth=180, suppress=False)

PID_GAINS = np.array([[40.0, 10.0, 0.001],  # [[400.0, 0.02, 1.6]
                      [120.0, 5.0, 0.05],    # [270.0, 0.02, 1.6]
                      [800.0, 8.0, 0.05],    # [200.0, 0.02, 0.6]
                      [1200.0, 10.0, 0.1],    # [200.0, 0.02, 0.5]
                      [2000.0, 20.0, 0.1],    # [120.0, 0.02, 0.3]
                      [1700.0, 40.0, 0.0]])   # [30.0, 0.02, 0.03]]
# ## Using LIMMS Robot
limms = robot.Limms("LIMMS1")
limms.set_parameters(np.flipud(PID_GAINS))
limms.update() # FK
fm = limms.forward_model

def fk(q):
    p = rbdl.CalcBodyToBaseCoordinates(fm.model, q, fm.body_id, fm.body_point, update_kinematics=True)
    R = rbdl.CalcBodyWorldOrientation(fm.model, q, fm.body_id, update_kinematics=False).T  # @ fm.R_correction

    return p, R

def get_box_trajectory(p0, v0, t):
    # p0, v0 (3, )
    return p0 + v0 * t - 0.5 * np.array([0.0, 0.0, 9.81]) * t **2

def IK_traj_opt(h=1, dt=0.1):
    h = 5
    dt = 0.01

    n_joint = len(limms.q)

    def obj(q, ps, Rn, c): # c: [close_q, early_p, final_p, final_R]
        cost = 0

        q_current = limms.q
        for hdx in range(h):
            cost += c[0] * np.linalg.norm(q[n_joint * hdx:n_joint * (hdx + 1)] - q_current)
            q_current = q[n_joint * hdx:n_joint * (hdx + 1)]

        c_p = np.linspace(c[1], c[2], h)
        for hdx in range(h):

            _q = q[n_joint*hdx:n_joint*(hdx+1)]
            p_fk, R_fk = fk(_q)
            p_des = ps[hdx, :]

            cost += c_p[hdx] * np.linalg.norm(p_des - p_fk)
            if hdx == h-1:
                eul_des = scipyr.Rotation.from_matrix(Rn).as_euler('zyx')
                eul_fk = scipyr.Rotation.from_matrix(R_fk).as_euler('zyx')
                cost += c[3] * np.linalg.norm(eul_des - eul_fk)

        return cost

    limms.update()
    # p0 = np.array([-0.5, -0.3, -0.5])
    q0 = limms.q.copy()
    p0 = limms.p_bf.copy()

    p1, Rd = fk(np.random.random((n_joint, )) * 2 * np.pi - np.pi, fm) # np.array([-0.5, -0.3, -0.5])
    ps = np.linspace(p0, p1, h)

    t0 = time.time()
    q_d = scipy.optimize.fmin_powell(obj, args=(ps, Rd, [1.0, 10.0, 100.0, 1.0]), x0=np.tile(limms.q.copy(), (h, )))
    print(time.time() - t0)

    # print(ps)
    # print(q0, q_d)
    # print(p1, fk(q_d[:-n_joint], fm)[0])

    for hdx in range(h):
        limms.update()
        _q_d = q_d[n_joint*hdx:n_joint*(hdx+1)]

        _q = np.linspace(limms.q, _q_d, int(1/dt))

        for qdx in range(_q.shape[0]):
            limms.set_command_position(_q[qdx])
            time.sleep(dt)

    # t = np.arange(0, h*int(1/dt)) * dt
    q_d = q_d.reshape((-1, n_joint))
    t = np.arange(0, q_d.shape[0])

    p_q = np.zeros((h, 3))
    for hdx in range(h):
        p_q[hdx, :] = fk(q_d[hdx], fm)[0]

    plt.figure(0)
    plt.subplot(311)
    # plt.plot(t, q_d[:, 0])
    plt.plot(t, p_q[:, 0])
    plt.plot(t[0], p0[0], 'r*')
    plt.plot(t[-1], p1[0], 'r*')
    plt.subplot(312)
    # plt.plot(t, q_d[:, 1])
    plt.plot(t, p_q[:, 1])
    plt.plot(t[0], p0[1], 'r*')
    plt.plot(t[-1], p1[1], 'r*')
    plt.subplot(313)
    # plt.plot(t, q_d[:, 2])
    plt.plot(t, p_q[:, 2])
    plt.plot(t[0], p0[2], 'r*')
    plt.plot(t[-1], p1[2], 'r*')
    plt.show()
    print("done")




if __name__ == '__main__':
    # IK_rbdl()
    # IK_opt()
    IK_traj_opt()
    print("Done!")